├── CAD └── CameraBot.zip ├── README.md ├── Arduino └── 005 │ ├── thresholdSticks.ino │ └── 005.ino ├── LICENSE └── Python └── camerabot01.py /CAD/CameraBot.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/XRobots/CameraBot/HEAD/CAD/CameraBot.zip -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CameraBot 2 | 3 | Auto tracking camera-bot: https://youtu.be/jouYRb70814 4 | -------------------------------------------------------------------------------- /Arduino/005/thresholdSticks.ino: -------------------------------------------------------------------------------- 1 | int thresholdStick (int pos) { 2 | 3 | // get zero centre position 4 | pos = pos - 512; 5 | 6 | // threshold value for control sticks 7 | if (pos > 100) { 8 | pos = pos -100; 9 | } 10 | else if (pos < -100) { 11 | pos = pos +100; 12 | } 13 | else { 14 | pos = 0; 15 | } 16 | 17 | return pos; 18 | } 19 | 20 | 21 | 22 | // motion filter to filter motions 23 | 24 | float filter(float prevValue, float currentValue, int filter) { 25 | float lengthFiltered = (prevValue + (currentValue * filter)) / (filter + 1); 26 | return lengthFiltered; 27 | } 28 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 James Bruton 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Python/camerabot01.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import serial 4 | 5 | import RPi.GPIO as GPIO 6 | import jetson.inference 7 | import jetson.utils 8 | 9 | import time 10 | 11 | import argparse 12 | import sys 13 | 14 | #setup serial 15 | arduino = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, timeout=.1) 16 | 17 | # parse the command line 18 | parser = argparse.ArgumentParser(description="Locate objects in a live camera stream using an object detection DNN.", 19 | formatter_class=argparse.RawTextHelpFormatter, epilog=jetson.inference.detectNet.Usage() + 20 | jetson.utils.videoSource.Usage() + jetson.utils.videoOutput.Usage() + jetson.utils.logUsage()) 21 | 22 | parser.add_argument("input_URI", type=str, default="", nargs='?', help="URI of the input stream") 23 | parser.add_argument("output_URI", type=str, default="", nargs='?', help="URI of the output stream") 24 | parser.add_argument("--network", type=str, default="ssd-mobilenet-v2", help="pre-trained model to load (see below for options)") 25 | parser.add_argument("--overlay", type=str, default="box,labels,conf", help="detection overlay flags (e.g. --overlay=box,labels,conf)\nvalid combinations are: 'box', 'labels', 'conf', 'none'") 26 | parser.add_argument("--threshold", type=float, default=0.5, help="minimum detection threshold to use") 27 | 28 | is_headless = ["--headless"] if sys.argv[0].find('console.py') != -1 else [""] 29 | 30 | try: 31 | opt = parser.parse_known_args()[0] 32 | except: 33 | print("") 34 | parser.print_help() 35 | sys.exit(0) 36 | 37 | # load the object detection network 38 | net = jetson.inference.detectNet(opt.network, sys.argv, opt.threshold) 39 | 40 | # create video sources & outputs 41 | input = jetson.utils.videoSource(opt.input_URI, argv=sys.argv) 42 | output = jetson.utils.videoOutput(opt.output_URI, argv=sys.argv+is_headless) 43 | 44 | # declare variables as global and that 45 | 46 | global index 47 | global width 48 | global location 49 | index = 0 50 | width = 0 51 | location = 0 52 | flag = 0 53 | 54 | # process frames until the user exits 55 | while True: 56 | 57 | # capture the next image 58 | img = input.Capture() 59 | 60 | # detect objects in the image (with overlay) 61 | detections = net.Detect(img, overlay=opt.overlay) 62 | 63 | # print the detections 64 | #print("detected {:d} objects in image".format(len(detections))) 65 | numberDet = (len(detections)) 66 | #print(numberDet) 67 | 68 | if numberDet > 0: 69 | 70 | for detection in detections: 71 | index = detections[0].ClassID 72 | width = int((detections[0].Width)) 73 | location1 = int((detections[0].Center[0])) 74 | location2 = int((detections[0].Top)) 75 | 76 | # print index of item, width and horizonal location 77 | if index == 1: 78 | print("detection:") 79 | print(index) 80 | print(width) 81 | print(location1) 82 | print(location2) 83 | chara = str(-20) 84 | char1 = str(width) 85 | char2 = str(location1) 86 | char3 = str(location2) 87 | arduino.write(bytes(chara, 'utf8')) 88 | arduino.write(bytes(',', 'utf8')) 89 | arduino.write(bytes(char1, 'utf8')) 90 | arduino.write(bytes(',', 'utf8')) 91 | arduino.write(bytes(char2, 'utf8')) 92 | arduino.write(bytes(',', 'utf8')) 93 | arduino.write(bytes(char3, 'utf8')) 94 | arduino.write(bytes(',', 'utf8')) 95 | else: 96 | print("no detection") 97 | chara = str(-40) 98 | arduino.write(bytes(chara, 'utf8')) 99 | arduino.write(bytes(',', 'utf8')) 100 | 101 | # render the image 102 | output.Render(img) 103 | 104 | # update the title bar 105 | output.SetStatus("{:s} | Network {:.0f} FPS".format(opt.network, net.GetNetworkFPS())) 106 | 107 | # print out performance info 108 | #net.PrintProfilerTimes() 109 | 110 | # exit on input/output EOS 111 | if not input.IsStreaming() or not output.IsStreaming(): 112 | break 113 | 114 | 115 | -------------------------------------------------------------------------------- /Arduino/005/005.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define DXL_SERIAL Serial3 4 | #define DEBUG_SERIAL Serial 5 | const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN 6 | 7 | const float DXL_PROTOCOL_VERSION = 2.0; 8 | 9 | #include // PID 10 | 11 | double Pk0 = 0.1; 12 | double Ik0 = 0.0; 13 | double Dk0 = 0.0025; 14 | 15 | double Setpoint0, Input0, Output0, Output0a; // PID variables 16 | PID PID0(&Input0, &Output0, &Setpoint0, Pk0, Ik0 , Dk0, DIRECT); // PID Setup 17 | 18 | double Pk1 = 0.1; 19 | double Ik1 = 0.0; 20 | double Dk1 = 0.05; 21 | 22 | double Setpoint1, Input1, Output1, Output1a; // PID variables 23 | PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT); // PID Setup 24 | 25 | double Pk2 = 0.1; 26 | double Ik2 = 0.0; 27 | double Dk2 = 0.1; 28 | 29 | double Setpoint2, Input2, Output2, Output2a; // PID variables 30 | PID PID2(&Input2, &Output2, &Setpoint2, Pk2, Ik2 , Dk2, DIRECT); // PID Setup 31 | 32 | int sw1; 33 | int sw2; 34 | int sw3; 35 | int sw4; 36 | int sw5; 37 | float pot1 = 512; 38 | float pot2 = 512; 39 | float pot3 = 512; 40 | float pot1Filtered = 512; 41 | float pot2Filtered = 512; 42 | float pot3Filtered = 512; 43 | float pot4; 44 | float pot5; 45 | float pot6; 46 | float pot4a; 47 | float pot5a; 48 | float pot6a; 49 | int pot4Scaled; 50 | int pot5Scaled; 51 | int pot6Scaled; 52 | 53 | int wheel1; 54 | int wheel2; 55 | int wheel3; 56 | int wheel1a; 57 | int wheel2a; 58 | int wheel3a; 59 | 60 | float dyn0 = 180; 61 | float dyn1 = 180; 62 | float dyn2 = 180; 63 | float dyn3 = 180; 64 | 65 | float dyn0Filtered = 180; 66 | float dyn1Filtered = 180; 67 | float dyn2Filtered = 180; 68 | float dyn2FilteredPrev = 180; 69 | float dyn3Filtered = 180; 70 | 71 | // serial vars: 72 | int var1; 73 | int var2; 74 | int var3; 75 | int check1; 76 | 77 | float var1a; 78 | float var2a; 79 | float var3a; 80 | 81 | float var1aFiltered; 82 | float var2aFiltered; 83 | float var3aFiltered; 84 | 85 | int foot1; 86 | int foot2; 87 | int foot3; 88 | int foot1Flag = 0; 89 | 90 | unsigned long currentMillis; 91 | long previousMillis = 0; // set up timers 92 | long interval = 10; // time constant for timer 93 | long previousNodMillis = 0; // set up timers 94 | long previousFoot1Millis = 0; // set up timers 95 | long previousHandMillis = 0; // set up timers 96 | int nodFlag = 0; 97 | int handFlag = 0; 98 | int nodDepth = 10; 99 | int nodTime = 250; 100 | 101 | int dyn0Bookmark; 102 | int dyn1Bookmark; 103 | int dyn2Bookmark; 104 | int dyn3Bookmark; 105 | int dyn0aBookmark; 106 | int dyn1aBookmark; 107 | int dyn2aBookmark; 108 | int dyn3aBookmark; 109 | int dyn0bBookmark; 110 | int dyn1bBookmark; 111 | int dyn2bBookmark; 112 | int dyn3bBookmark; 113 | 114 | Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); 115 | 116 | //This namespace is required to use Control table item names 117 | using namespace ControlTableItem; 118 | 119 | void setup() { 120 | 121 | pinMode(22, INPUT_PULLUP); // switches 122 | pinMode(24, INPUT_PULLUP); 123 | pinMode(26, INPUT_PULLUP); 124 | pinMode(28, INPUT_PULLUP); 125 | pinMode(30, INPUT_PULLUP); 126 | 127 | pinMode(53, INPUT_PULLUP); // footswitches 128 | pinMode(51, INPUT_PULLUP); 129 | pinMode(49, INPUT_PULLUP); 130 | 131 | pinMode(3, OUTPUT); // motor PWMs 132 | pinMode(4, OUTPUT); 133 | pinMode(5, OUTPUT); 134 | pinMode(8, OUTPUT); 135 | pinMode(9, OUTPUT); 136 | pinMode(10, OUTPUT); 137 | 138 | // Use UART port of DYNAMIXEL Shield to debug. 139 | DEBUG_SERIAL.begin(115200); 140 | 141 | // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. 142 | dxl.begin(115200); 143 | // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. 144 | dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); 145 | // Get DYNAMIXEL information 146 | dxl.ping(0); 147 | dxl.ping(1); 148 | dxl.ping(2); 149 | dxl.ping(3); 150 | 151 | // Turn off torque when configuring items in EEPROM area 152 | dxl.torqueOff(0); 153 | dxl.torqueOff(1); 154 | dxl.torqueOff(2); 155 | dxl.torqueOff(3); 156 | dxl.setOperatingMode(0, OP_POSITION); 157 | dxl.setOperatingMode(1, OP_POSITION); 158 | dxl.setOperatingMode(2, OP_POSITION); 159 | dxl.setOperatingMode(3, OP_POSITION); 160 | dxl.torqueOn(0); 161 | dxl.torqueOn(1); 162 | dxl.torqueOn(2); 163 | dxl.torqueOn(3); 164 | 165 | Serial2.begin(115200); // serial comms to Nano 166 | 167 | PID0.SetMode(AUTOMATIC); 168 | PID0.SetOutputLimits(-390, 390); 169 | PID0.SetSampleTime(10); 170 | 171 | PID1.SetMode(AUTOMATIC); 172 | PID1.SetOutputLimits(-640, 640); 173 | PID1.SetSampleTime(10); 174 | 175 | PID2.SetMode(AUTOMATIC); 176 | PID2.SetOutputLimits(-640, 640); 177 | PID2.SetSampleTime(10); 178 | 179 | // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed 180 | dxl.writeControlTableItem(PROFILE_VELOCITY, 0, 120); // pan 181 | dxl.writeControlTableItem(PROFILE_VELOCITY, 2, 65); // arm 182 | dxl.writeControlTableItem(PROFILE_VELOCITY, 1, 120); // tilt 183 | dxl.writeControlTableItem(PROFILE_VELOCITY, 3, 120); // zoom 184 | 185 | dxl.setGoalPosition(2, 180, UNIT_DEGREE); // arm 186 | } 187 | 188 | void loop() { 189 | 190 | currentMillis = millis(); 191 | if (currentMillis - previousMillis >= interval) { // start timed event 192 | previousMillis = currentMillis; 193 | 194 | 195 | if (Serial2.available() > 1){ 196 | check1 = Serial2.parseInt(); 197 | if (check1 == -20) { //wait for the check value to come around before reading the rest of the data 198 | var1 = Serial2.parseInt(); // detection width 199 | var2 = Serial2.parseInt(); // left to right 200 | var3 = Serial2.parseInt(); // top 201 | if (var1 > 300) { // filter out small items in background 202 | var1a = float(var1); 203 | var2a = float(var2); 204 | var3a = float(var3); 205 | } 206 | } 207 | } 208 | 209 | pot1 = analogRead(A8); 210 | pot2 = analogRead(A9); 211 | pot3 = analogRead(A10); 212 | pot4 = analogRead(A11); 213 | pot5 = analogRead(A12); 214 | pot6 = analogRead(A13); 215 | 216 | foot1 = digitalRead(49); 217 | foot2 = digitalRead(51); 218 | foot3 = digitalRead(53); 219 | 220 | pot1Filtered = filter(pot1, pot1Filtered,30); 221 | pot2Filtered = filter(pot2, pot2Filtered,30); 222 | pot3Filtered = filter(pot3, pot3Filtered,30); 223 | 224 | pot4a = thresholdStick(pot4); // manual driving knobs 225 | pot5a = thresholdStick(pot5); 226 | pot6a = thresholdStick(pot6); 227 | 228 | pot4Scaled = map(pot4a,-512,512,255,-255); 229 | pot5Scaled = map(pot5a,-512,512,255,-255); 230 | pot6Scaled = map(pot6a,-512,512,-255,255); 231 | 232 | sw1 = digitalRead(22); 233 | sw2 = digitalRead(24); 234 | sw3 = digitalRead(26); 235 | sw4 = digitalRead(28); 236 | sw5 = digitalRead(30); 237 | 238 | var1aFiltered = filter(var1a, var1aFiltered, 50); 239 | var2aFiltered = filter(var2a, var2aFiltered, 30); 240 | var3aFiltered = filter(var3a, var3aFiltered, 30); 241 | 242 | if (sw1 == 0) { /// vision tracking 243 | 244 | //pan 245 | 246 | Input1 = var2aFiltered; // centre horiz 247 | Setpoint1 = 640; 248 | PID1.Compute(); 249 | dyn0 = dyn0 + (Output1/300); 250 | dyn0 = constrain(dyn0,135,225); 251 | 252 | // tilt 253 | Input2 = var3aFiltered-300; // top 254 | Setpoint2 = -200; 255 | PID2.Compute(); 256 | Output2 = Output2 + 100; // create and remove offsets so there is equal swing around zero - otherwise the distance between the top of the detection is smaller at the top. 257 | Output2 = map(Output2,0,100,-100,000); 258 | dyn1 = dyn1 - (Output2/300); 259 | dyn1 = constrain(dyn1,135,225); 260 | 261 | // zoom 262 | Input0 = var1aFiltered - 1000; // width 263 | Setpoint0 = 0; 264 | PID0.Compute(); 265 | dyn3 = Output0+50; 266 | dyn3 = map(dyn3,60,290,290,60); 267 | dyn3 = constrain(dyn3,60,290); 268 | 269 | } 270 | 271 | else if (sw1 == 1) { // use knobs in front to fix position 272 | dyn0 = map(pot1Filtered,0,1023,135,225); 273 | dyn1 = map(pot2Filtered,0,1023,135,225); 274 | dyn3 = map(pot3Filtered,0,1023,60,290); 275 | } 276 | 277 | // footswitch - look away to preset position 278 | 279 | if (foot1 == 0 && foot1Flag == 0 ) { // boommark current positions 280 | dyn0Bookmark = dyn0; 281 | dyn1Bookmark = dyn1; 282 | dyn3Bookmark = dyn3; 283 | previousFoot1Millis = currentMillis; 284 | foot1Flag = 1; 285 | } 286 | 287 | else if (foot1Flag == 1) { // go to preset positions 288 | dyn0 = map(pot1Filtered,0,1023,135,225); 289 | dyn1 = map(pot2Filtered,0,1023,135,225); 290 | dyn3 = map(pot3Filtered,0,1023,60,290); 291 | if (foot1 == 0 && currentMillis - previousFoot1Millis >= 500) { 292 | foot1Flag = 2; 293 | previousFoot1Millis = currentMillis; 294 | } 295 | } 296 | 297 | else if (foot1Flag == 2) { // go back to the book marks. 298 | dyn0 = dyn0Bookmark; 299 | dyn1 = dyn1Bookmark; 300 | dyn3 = dyn3Bookmark; 301 | if (currentMillis - previousFoot1Millis >= 1500) { 302 | foot1Flag = 0; 303 | } 304 | } 305 | 306 | Serial.println(foot1Flag); 307 | 308 | // nod for yes when footswitch 2 is pressed 309 | 310 | if (foot2 == 0 && nodFlag == 0) { 311 | previousNodMillis = currentMillis; 312 | dyn1aBookmark = dyn1; 313 | dyn1 = dyn1aBookmark + nodDepth; 314 | nodFlag = 1; 315 | } 316 | else if (nodFlag == 1 && currentMillis - previousNodMillis >= nodTime) { 317 | previousNodMillis = currentMillis; 318 | dyn1 = dyn1aBookmark - nodDepth; 319 | nodFlag = 2; 320 | } 321 | else if (nodFlag == 2 && currentMillis - previousNodMillis >= nodTime) { 322 | previousNodMillis = currentMillis; 323 | dyn1 = dyn1aBookmark + nodDepth; 324 | nodFlag = 3; 325 | } 326 | else if (nodFlag == 3 && currentMillis - previousNodMillis >= nodTime) { 327 | previousNodMillis = currentMillis; 328 | dyn1 = dyn1aBookmark - nodDepth; 329 | nodFlag = 4; 330 | } 331 | else if (nodFlag == 4 && currentMillis - previousNodMillis >= nodTime) { 332 | previousNodMillis = currentMillis; 333 | dyn1 = dyn1aBookmark + nodDepth; 334 | nodFlag = 5; 335 | } 336 | else if (nodFlag == 5 && currentMillis - previousNodMillis >= nodTime) { 337 | previousNodMillis = currentMillis; 338 | dyn1 = dyn1aBookmark - nodDepth; 339 | nodFlag = 6; 340 | } 341 | else if (nodFlag == 6 && currentMillis - previousNodMillis >= nodTime) { 342 | previousNodMillis = currentMillis; 343 | dyn1 = dyn1aBookmark + nodDepth; 344 | nodFlag = 7; 345 | } 346 | else if (nodFlag == 7 && currentMillis - previousNodMillis >= nodTime) { 347 | dyn1 = dyn1aBookmark; 348 | if (currentMillis - previousNodMillis >= nodTime) { 349 | nodFlag = 0; 350 | } 351 | } 352 | 353 | 354 | 355 | 356 | // *** thumbs up 357 | 358 | if (foot3 == 0 && handFlag == 0) { 359 | dyn0bBookmark = dyn0; 360 | dyn1bBookmark = dyn1; 361 | dyn3bBookmark = dyn3; 362 | handFlag = 1; 363 | previousHandMillis = currentMillis; 364 | } 365 | else if (handFlag == 1) { 366 | dyn2 = 75; // arm 367 | dyn3 = 290; // zoom 368 | dyn0 = dyn0bBookmark; 369 | dyn1 = dyn1bBookmark; 370 | if (currentMillis - previousHandMillis >= 2000) { 371 | dyn2 = 180; 372 | dyn3 = dyn3bBookmark; 373 | handFlag = 0; 374 | } 375 | } 376 | 377 | // write to servos 378 | 379 | dyn0Filtered = filter(dyn0, dyn0Filtered, 10); 380 | dyn1Filtered = filter(dyn1, dyn1Filtered, 10); 381 | dyn3Filtered = filter(dyn3, dyn3Filtered, 10); 382 | dyn2Filtered = filter(dyn2, dyn2Filtered, 25); /// arm 383 | 384 | // *** write to dynamixels 385 | 386 | if (dyn2Filtered != dyn2FilteredPrev) { 387 | dxl.setGoalPosition(2, dyn2Filtered, UNIT_DEGREE); // only write to arm if the value changes 388 | } 389 | dyn2FilteredPrev = dyn2Filtered; 390 | 391 | dxl.setGoalPosition(1, dyn1Filtered, UNIT_DEGREE); 392 | dxl.setGoalPosition(3, dyn3Filtered, UNIT_DEGREE); 393 | 394 | if (sw2 == 1) { // operate pan servo 395 | dxl.setGoalPosition(0, dyn0Filtered, UNIT_DEGREE); 396 | } 397 | 398 | // *** use the wheels for transaltion instead of pan *** 399 | 400 | else if (sw2 == 0) { 401 | dxl.setGoalPosition(0, 180, UNIT_DEGREE); // fix pan servo in the middle 402 | pot5Scaled = map(var2aFiltered, 0,1280,-512,512); 403 | if (pot5Scaled > 100) { 404 | pot5Scaled = pot5Scaled - 100; // deadspot 405 | } 406 | else if (pot5Scaled < -100) { 407 | pot5Scaled = pot5Scaled + 100; 408 | } 409 | else { 410 | pot5Scaled = 0; 411 | } 412 | pot5Scaled = constrain(pot5Scaled,-255,255); 413 | } 414 | 415 | // *** drive motors *** 416 | 417 | wheel1 = pot4Scaled - (pot5Scaled*.66) - pot6Scaled; 418 | wheel3 = pot4Scaled + (pot5Scaled*.66) + pot6Scaled; 419 | wheel2 = pot5Scaled - pot6Scaled; 420 | 421 | wheel1 = constrain(wheel1,-255,255); 422 | wheel2 = constrain(wheel2,-255,255); 423 | wheel3 = constrain(wheel3,-255,255); 424 | 425 | if (sw3 == 0) { // allow motors to move 426 | 427 | if (wheel1 > 0) { 428 | analogWrite(10, wheel1); 429 | analogWrite(3, 0); 430 | } 431 | else if (wheel1 < 0) { 432 | wheel1a = abs(wheel1); 433 | analogWrite(3, wheel1a); 434 | analogWrite(10, 0); 435 | } 436 | else { 437 | analogWrite(3, 0); 438 | analogWrite(10, 0); 439 | } 440 | 441 | // ***** 442 | 443 | if (wheel2 > 0) { 444 | analogWrite(4, wheel2); 445 | analogWrite(5, 0); 446 | } 447 | else if (wheel2 < 0) { 448 | wheel2a = abs(wheel2); 449 | analogWrite(5, wheel2a); 450 | analogWrite(4, 0); 451 | } 452 | else { 453 | analogWrite(4, 0); 454 | analogWrite(5, 0); 455 | } 456 | 457 | // ***** 458 | 459 | if (wheel3 > 0) { 460 | analogWrite(8, wheel3); 461 | analogWrite(9, 0); 462 | } 463 | else if (wheel3 < 0) { 464 | wheel3a = abs(wheel3); 465 | analogWrite(9, wheel3a); 466 | analogWrite(8, 0); 467 | } 468 | else { 469 | analogWrite(8, 0); 470 | analogWrite(9, 0); 471 | } 472 | 473 | } 474 | else if (sw2 == 1) { 475 | analogWrite(3, 0); 476 | analogWrite(10, 0); 477 | 478 | analogWrite(4, 0); 479 | analogWrite(5, 0); 480 | 481 | analogWrite(8, 0); 482 | analogWrite(9, 0); 483 | } 484 | 485 | 486 | } // end of 10ms timed loop 487 | 488 | } // end of main 489 | 490 | 491 | --------------------------------------------------------------------------------