Components and supplies
Seeed Studio Shield Bot 1.2
Arduino Due
Apps and platforms
Arduino IDE
Project description
Code
Sketch Autonomous line follower with ARTe
arduino
This sketch, written for arduino due, allows to use the shieldbot v1.2 with two modes, that is in automatic mode and in manual mode. The rover will then be controlled via serial.
1/* 2 * The goal of this firmware is to control the engines of the rover through sensors or via commands that arrive via serial. 3 4 * We have two main modalities 5 AUTOMATIC. In this mode the sensors are read on board the rover and this will try to follow it. 6 MANUAL. In this mode instead, rover receives commands from the serial and we don't care About the values of the sensors, therefore the sensors will not be checked. 7 8 * Technically we have another mode, that is STOPPED. Rover enters in this modality only if we were on the AUTOMATIC mode before. 9 In this mode the rover locks the engines and waits to recive the CMD_RESTART command to restart in AUTOMATIC mode. 10 11 * In any of the three modes we find ourselves, it is always possible to receive the CHANGEMODALITY command, so we switch: 12 If we are in AUTOMATIC mode, then go to MANUAL mode 13 If we are in MANUAL mode, then go to AUTOMATIC mode 14 If we are in STOPPED mode, ther go to MANUAL mode 15 16 17 * MANUAL command (recived from users via serial) 18 It is possible to make the rover go forward, go back, go left and go right. 19 It is possible to modify the speed, at any time 20 To go forward send key '8' 21 To go backwards send key '2' 22 To go right send key '6' 23 To go left send key '4' 24 25 * Modify speed 26 Only in STOPPED mode it is not possible to modify the speed. 27 To increase speed send key '7' 28 To decrease speed send key '1' 29*/ 30#include "DriverMotors.h" 31 32#define SENSORS loop1 33#define MOTORS loop2 34#define COMMUNICATION loop3 35 36 37#define sprint Serial.print 38#define sprintln Serial.println 39 40//modes: 41#define AUTOMATIC '1' 42#define MANUAL '2' 43#define STOPPED '3' 44 45//commands: 46#define CHANGEMODALITY 'c' 47#define CMD_STOP '5' 48#define RESTART 'r' 49 50#define CMD_FORWARD '8' 51#define CMD_BACKWARDS '2' 52#define CMD_LEFT '4' 53#define CMD_RIGHT '6' 54#define CMD_MOTORUP '7' 55#define CMD_MOTORDOWN '1' 56 57//We decide to change the speed of these values. Range is: [0, 255]. Values outside this range will be changed automatically from "DriverMotors.h" 58#define INCREASE_MOTOR 20 59#define DECREASE_MOTOR 20 60 61 62/** 63 * On "S1, S2, .., S5" we save the values read by the sensors. 64 * On "mode" the mode is saved 65 * On "actualCommand" we save the last command received 66*/ 67static int S1, S2, S3, S4, S5; 68static char mode; 69static char actualCommand; 70 71 72 73void setup() { 74 Serial.begin(9600); 75 arteLock(); 76 S1 = S2 = S3 = S4 = S5 = HIGH; 77 arteUnlock(); 78 actualCommand = CMD_STOP; 79 80 mode = getCommand(); 81 mode = mode == AUTOMATIC ? AUTOMATIC : MANUAL; 82 initialize(140, 140); 83} 84 85/* 86 * Read the serial, and stands still until we receive a command 87*/ 88char getCommand() { 89 char c; 90 while (1){ 91 while (Serial.available()<=0); //Block the execution 92 c = Serial.read(); 93 if (c != '\n') break; 94 } 95 return c; 96} 97 98/* 99 * This function moves the rover along the path (the path is a black line) and sends to the serial the direction in which it's going. 100 * It tries to move in such a way the S3 sensor detects LOW, and the other sensors detect HIGH. 101 * 102 * SENSORS Eg. Ope path Eg. Close path 103 * || . (END) _______(START)/(END) 104 * S3 / | | 105 * S2 || S4 \\__ | | 106 * S1 || S5 \\__. (START) |_______| 107 * 108 * If the sensor don't detect any path (they don't find black under themselves) the rover will try to find a path backwards 109 * This because: 110 * If we are in an open path (see above) then the rover will not run away, but will go back and forth cyclically 111 * If during a difficult curve the rover goes too far because it is going too fast, it will come back to find the lost line 112*/ 113void follow_line() { 114 int s1, s2, s3, s4, s5; 115 arteLock(); //Start atomic section 116 s1 = S1; 117 s2 = S2; 118 s3 = S3; 119 s4 = S4; 120 s5 = S5; 121 arteUnlock(); //Finish atomic section 122 123 if ((s3==LOW) && (s1==HIGH && s2==HIGH && s4==HIGH && s5==HIGH)){ 124 sprintln(CMD_FORWARD); 125 goForward(); 126 } else { 127 if (s1==LOW || s2==LOW) { 128 sprintln (CMD_RIGHT); 129 goRight(); 130 131 } else if (s5==LOW || s4==LOW){ 132 sprintln (CMD_LEFT); 133 goLeft(); 134 135 } else if (s1 == HIGH && s2 == HIGH && s3 == HIGH && s4 == HIGH && s5 == HIGH) { 136 goBackwards(); 137 sprintln (CMD_BACKWARDS); 138 139 } else { 140 Serial.print("We will never enter in this condition\ 141"); 142 Serial.print("-1\ 143"); 144 145 stop(); 146 while (1) delay(100); 147 } 148 } 149} 150 151/* 152 * This function controls the rover 153 * Sends the command executed on the serial line 154*/ 155char driveRover(char cmd) { 156 switch(cmd) { 157 case CMD_FORWARD: 158 sprintln (CMD_FORWARD); 159 goForward(); 160 break; 161 case CMD_RIGHT: 162 sprintln (CMD_RIGHT); 163 goRight(); 164 break; 165 case CMD_LEFT: 166 sprintln (CMD_LEFT); 167 goLeft(); 168 break; 169 case CMD_BACKWARDS: 170 sprintln (CMD_BACKWARDS); 171 goBackwards(); 172 break; 173 case CMD_MOTORUP: 174 sprintln (CMD_MOTORUP); 175 increaseSpeed(INCREASE_MOTOR); 176 return ' '; 177 case CMD_MOTORDOWN: 178 sprintln (CMD_MOTORDOWN); 179 decreaseSpeed(DECREASE_MOTOR); 180 return ' '; 181 case CMD_STOP: 182 sprintln (CMD_STOP); 183 stop(); 184 break; 185 default: 186 sprint("[driveRover] default: <"); sprint(cmd); sprint("#>\ 187"); 188 return ' '; 189 } 190 return cmd; 191} 192 193/* 194 * This function is only called if we are in MANUAL mode. Sends via serial the current command that is running 195*/ 196void sendAction(char cmd) { 197 switch(cmd) { 198 case CMD_FORWARD: 199 sprintln (CMD_FORWARD); 200 break; 201 case CMD_RIGHT: 202 sprintln (CMD_RIGHT); 203 break; 204 case CMD_LEFT: 205 sprintln (CMD_LEFT); 206 break; 207 case CMD_BACKWARDS: 208 sprintln (CMD_BACKWARDS); 209 break; 210 case CMD_STOP: 211 sprintln (CMD_STOP); 212 break; 213 default: /*Not verifiable*/ 214 sprintln("-1"); 215 } 216} 217 218 219/* 220 * Read and store the values of sensors 221 */ 222void SENSORS(50) { 223 if (mode == AUTOMATIC) { 224 arteLock(); //Start atomic section 225 S1 = readS1(); 226 S2 = readS2(); 227 S3 = readS3(); 228 S4 = readS4(); 229 S5 = readS5(); 230 arteUnlock(); //Finish atomic section 231 } 232} 233 234/* 235 * Thread MOTORS: change its action based on the mode 236 * It must be able to move the rover only if we are in AUTOMATIC mode or MANUAL mode, else keep still the rover. 237 */ 238void MOTORS(100) { 239 char cmd; 240 switch (mode) { 241 case STOPPED: 242 stop(); 243 sprint(CMD_STOP); 244 break; 245 case AUTOMATIC: 246 follow_line(); 247 break; 248 case MANUAL: 249 arteLock(); 250 cmd = actualCommand; 251 arteUnlock(); 252 sendAction(cmd); 253 break; 254 default: /*Not verifiable*/ 255 sprintln("-1"); 256 } 257} 258 259/* 260 * This task receive command from the serial and will call the corresponding management routine 261 */ 262void COMMUNICATION(200) { 263 char cmd = getCommand(); 264 switch (mode) { 265 case AUTOMATIC: 266 if (cmd == CMD_STOP) { 267 mode = STOPPED; 268 269 } else if (cmd == CHANGEMODALITY) { 270 mode = MANUAL; 271 stop(); 272 273 } else if (cmd == CMD_MOTORUP) { 274 increaseSpeed(20); 275 276 } else if (cmd == CMD_MOTORDOWN) { 277 decreaseSpeed(20); 278 279 } 280 break; 281 case MANUAL: 282 if (cmd == CHANGEMODALITY) { 283 mode = AUTOMATIC; 284 } else { 285 cmd = driveRover(cmd); 286 if (cmd != ' ') { 287 arteLock(); //Start atomic section 288 actualCommand = cmd; 289 arteUnlock(); //Finish atomic section 290 } 291 } 292 break; 293 case STOPPED: 294 if (cmd == CHANGEMODALITY) 295 mode = MANUAL; 296 else if (cmd == RESTART) 297 mode = AUTOMATIC; 298 break; 299 default: /*Not verifiable*/ 300 sprintln("-1"); 301 } 302} 303 304void loop() { 305} 306
Sketch Autonomous line follower with ARTe
arduino
This sketch, written for arduino due, allows to use the shieldbot v1.2 with two modes, that is in automatic mode and in manual mode. The rover will then be controlled via serial.
1/* 2 * The goal of this firmware is to control the engines of the 3 rover through sensors or via commands that arrive via serial. 4 5 * We 6 have two main modalities 7 AUTOMATIC. In this mode the sensors are read 8 on board the rover and this will try to follow it. 9 MANUAL. In this mode 10 instead, rover receives commands from the serial and we don't care About the values 11 of the sensors, therefore the sensors will not be checked. 12 13 * Technically 14 we have another mode, that is STOPPED. Rover enters in this modality only if we 15 were on the AUTOMATIC mode before. 16 In this mode the rover locks the engines 17 and waits to recive the CMD_RESTART command to restart in AUTOMATIC mode. 18 19 20 * In any of the three modes we find ourselves, it is always possible to receive 21 the CHANGEMODALITY command, so we switch: 22 If we are in AUTOMATIC mode, 23 then go to MANUAL mode 24 If we are in MANUAL mode, then go to AUTOMATIC 25 mode 26 If we are in STOPPED mode, ther go to MANUAL mode 27 28 29 30 * MANUAL command (recived from users via serial) 31 It is possible to make 32 the rover go forward, go back, go left and go right. 33 It is possible to modify 34 the speed, at any time 35 To go forward send key '8' 36 To go backwards 37 send key '2' 38 To go right send key '6' 39 To go left send key 40 '4' 41 42 * Modify speed 43 Only in STOPPED mode it is not possible to modify 44 the speed. 45 To increase speed send key '7' 46 To decrease speed 47 send key '1' 48*/ 49#include "DriverMotors.h" 50 51#define SENSORS loop1 52#define 53 MOTORS loop2 54#define COMMUNICATION loop3 55 56 57#define sprint 58 Serial.print 59#define sprintln Serial.println 60 61//modes: 62#define 63 AUTOMATIC '1' 64#define MANUAL '2' 65#define STOPPED '3' 66 67//commands: 68#define 69 CHANGEMODALITY 'c' 70#define CMD_STOP '5' 71#define RESTART 'r' 72 73#define 74 CMD_FORWARD '8' 75#define CMD_BACKWARDS '2' 76#define CMD_LEFT '4' 77#define 78 CMD_RIGHT '6' 79#define CMD_MOTORUP '7' 80#define CMD_MOTORDOWN '1' 81 82//We 83 decide to change the speed of these values. Range is: [0, 255]. Values outside this 84 range will be changed automatically from "DriverMotors.h" 85#define INCREASE_MOTOR 86 20 87#define DECREASE_MOTOR 20 88 89 90/** 91 * On "S1, S2, .., S5" we 92 save the values read by the sensors. 93 * On "mode" the mode is saved 94 * 95 On "actualCommand" we save the last command received 96*/ 97static int S1, 98 S2, S3, S4, S5; 99static char mode; 100static char actualCommand; 101 102 103 104void 105 setup() { 106 Serial.begin(9600); 107 arteLock(); 108 S1 = S2 = S3 = S4 109 = S5 = HIGH; 110 arteUnlock(); 111 actualCommand = CMD_STOP; 112 113 mode 114 = getCommand(); 115 mode = mode == AUTOMATIC ? AUTOMATIC : MANUAL; 116 initialize(140, 117 140); 118} 119 120/* 121 * Read the serial, and stands still until we receive a 122 command 123*/ 124char getCommand() { 125 char c; 126 while (1){ 127 while 128 (Serial.available()<=0); //Block the execution 129 c = Serial.read(); 130 131 if (c != '\ 132') break; 133 } 134 return c; 135} 136 137/* 138 * This 139 function moves the rover along the path (the path is a black line) and sends to 140 the serial the direction in which it's going. 141 * It tries to move in such a way 142 the S3 sensor detects LOW, and the other sensors detect HIGH. 143 * 144 * SENSORS 145 Eg. Ope path Eg. 146 Close path 147 * || . 148 (END) _______(START)/(END) 149 * S3 150 / | 151 | 152 * S2 || S4 \\__ 153 | | 154 * S1 || S5 155 \\__. (START) |_______| 156 157 * 158 * If the sensor don't detect any path (they don't find black under themselves) 159 the rover will try to find a path backwards 160 * This because: 161 * If we are 162 in an open path (see above) then the rover will not run away, but will go back and 163 forth cyclically 164 * If during a difficult curve the rover goes too far because 165 it is going too fast, it will come back to find the lost line 166*/ 167void follow_line() 168 { 169 int s1, s2, s3, s4, s5; 170 arteLock(); //Start atomic section 171 172 s1 = S1; 173 s2 = S2; 174 s3 = S3; 175 s4 = S4; 176 s5 = S5; 177 178 arteUnlock(); //Finish atomic section 179 180 if ((s3==LOW) && 181 (s1==HIGH && s2==HIGH && s4==HIGH && s5==HIGH)){ 182 sprintln(CMD_FORWARD); 183 184 goForward(); 185 } else { 186 if (s1==LOW || s2==LOW) { 187 sprintln 188 (CMD_RIGHT); 189 goRight(); 190 191 } else if (s5==LOW || s4==LOW){ 192 193 sprintln (CMD_LEFT); 194 goLeft(); 195 196 } else 197 if (s1 == HIGH && s2 == HIGH && s3 == HIGH && s4 == HIGH && s5 == HIGH) { 198 goBackwards(); 199 200 sprintln (CMD_BACKWARDS); 201 202 } else { 203 Serial.print("We 204 will never enter in this condition\ 205"); 206 Serial.print("-1\ 207"); 208 209 210 stop(); 211 while (1) delay(100); 212 } 213 214 } 215} 216 217/* 218 * This function controls the rover 219 * Sends the command 220 executed on the serial line 221*/ 222char driveRover(char cmd) { 223 switch(cmd) 224 { 225 case CMD_FORWARD: 226 sprintln (CMD_FORWARD); 227 goForward(); 228 229 break; 230 case CMD_RIGHT: 231 sprintln (CMD_RIGHT); 232 233 goRight(); 234 break; 235 case CMD_LEFT: 236 sprintln 237 (CMD_LEFT); 238 goLeft(); 239 break; 240 case CMD_BACKWARDS: 241 242 sprintln (CMD_BACKWARDS); 243 goBackwards(); 244 break; 245 246 case CMD_MOTORUP: 247 sprintln (CMD_MOTORUP); 248 increaseSpeed(INCREASE_MOTOR); 249 250 return ' '; 251 case CMD_MOTORDOWN: 252 sprintln (CMD_MOTORDOWN); 253 254 decreaseSpeed(DECREASE_MOTOR); 255 return ' '; 256 case 257 CMD_STOP: 258 sprintln (CMD_STOP); 259 stop(); 260 break; 261 262 default: 263 sprint("[driveRover] default: <"); sprint(cmd); 264 sprint("#>\ 265"); 266 return ' '; 267 } 268 return cmd; 269} 270 271/* 272 273 * This function is only called if we are in MANUAL mode. Sends via serial the 274 current command that is running 275*/ 276void sendAction(char cmd) { 277 switch(cmd) 278 { 279 case CMD_FORWARD: 280 sprintln (CMD_FORWARD); 281 break; 282 283 case CMD_RIGHT: 284 sprintln (CMD_RIGHT); 285 break; 286 287 case CMD_LEFT: 288 sprintln (CMD_LEFT); 289 break; 290 291 case CMD_BACKWARDS: 292 sprintln (CMD_BACKWARDS); 293 break; 294 295 case CMD_STOP: 296 sprintln (CMD_STOP); 297 break; 298 299 default: /*Not verifiable*/ 300 sprintln("-1"); 301 } 302 303} 304 305 306/* 307 * Read and store the values of sensors 308 */ 309void SENSORS(50) 310 { 311 if (mode == AUTOMATIC) { 312 arteLock(); //Start atomic 313 section 314 S1 = readS1(); 315 S2 = readS2(); 316 S3 = readS3(); 317 318 S4 = readS4(); 319 S5 = readS5(); 320 arteUnlock(); //Finish 321 atomic section 322 } 323} 324 325/* 326 * Thread MOTORS: change its action based 327 on the mode 328 * It must be able to move the rover only if we are in AUTOMATIC 329 mode or MANUAL mode, else keep still the rover. 330 */ 331void MOTORS(100) { 332 333 char cmd; 334 switch (mode) { 335 case STOPPED: 336 stop(); 337 338 sprint(CMD_STOP); 339 break; 340 case AUTOMATIC: 341 342 follow_line(); 343 break; 344 case MANUAL: 345 arteLock(); 346 347 cmd = actualCommand; 348 arteUnlock(); 349 sendAction(cmd); 350 351 break; 352 default: /*Not verifiable*/ 353 sprintln("-1"); 354 355 } 356} 357 358/* 359 * This task receive command from the serial and will call 360 the corresponding management routine 361 */ 362void COMMUNICATION(200) { 363 char 364 cmd = getCommand(); 365 switch (mode) { 366 case AUTOMATIC: 367 if 368 (cmd == CMD_STOP) { 369 mode = STOPPED; 370 371 } 372 else if (cmd == CHANGEMODALITY) { 373 mode = MANUAL; 374 stop(); 375 376 377 } else if (cmd == CMD_MOTORUP) { 378 increaseSpeed(20); 379 380 381 } else if (cmd == CMD_MOTORDOWN) { 382 decreaseSpeed(20); 383 384 385 } 386 break; 387 case MANUAL: 388 if (cmd 389 == CHANGEMODALITY) { 390 mode = AUTOMATIC; 391 } else 392 { 393 cmd = driveRover(cmd); 394 if (cmd != ' ') { 395 396 arteLock(); //Start atomic section 397 actualCommand 398 = cmd; 399 arteUnlock(); //Finish atomic section 400 401 } 402 } 403 break; 404 case STOPPED: 405 406 if (cmd == CHANGEMODALITY) 407 mode = MANUAL; 408 else 409 if (cmd == RESTART) 410 mode = AUTOMATIC; 411 break; 412 413 default: /*Not verifiable*/ 414 sprintln("-1"); 415 } 416} 417 418void 419 loop() { 420} 421
firmware motors
c_cpp
With this software you can use this shieldbot easily: http://wiki.seeedstudio.com/Shield_Bot_V1.2/
1/* 2 * Motor control. 3*/ 4 5#ifndef DRIVERMOTORS_H 6#define DRIVERMOTORS_H 7 8#include <Arduino.h> 9 10 11#define RIGHT1 8 12#define SPEEDPINRIGHT 9 13#define RIGHT2 11 14#define LEFT1 12 15#define SPEEDPINLEFT 10 16#define LEFT2 13 17 18#define FINDER1 A0 19#define FINDER2 A1 20#define FINDER3 A2 21#define FINDER4 A3 22#define FINDER5 4 /*This pin is 4, not A4*/ 23 24#define MAXSPEED 255 25#define MINSPEED 100 26#define MINSPEED 0 27#define MAXSPEEDREVERSE -255 28 29#define FORWARD 1 30#define STOP 0 31#define BACKWARDS -1 32 33#define print Serial.print 34#define println Serial.println 35#define PRINTDEBUG print("\ [DEBUG] motordx: "); println(speedmotorRight); \\ 36 print("\ [DEBUG] motorsx: "); println(speedmotorLeft); 37 38//**************************************************************************************** 39// Global variables 40//**************************************************************************************** 41 42int speedmotorLeft; //Power of the left engine. This value will be given to the analogWrite function. The range is: [0:1:255] 43int speedmotorRight; //Power of the right engine. This value will be given to the analogWrite function. The range is: [0:1:255] 44int initValueMotorLeft; //Initial and minimum values of the engine. Is not possible to exceed these ranges of values 45int initValueMotorRight; 46int directionMotorLeft; 47int directionMotorRight; 48 49 50//**************************************************************************************** 51// Prototypes 52//**************************************************************************************** 53 54int readS1(); 55int readS2(); 56int readS3(); 57int readS4(); 58int readS5(); 59int getSpeed(); 60void goLeft(); 61void goRight(); 62void initialize(int min_speed_left, int min_speed_right); 63void increaseSpeed(int powerOffset); 64void decreaseSpeed(int powerOffset); 65void increaseSpeedLeft(int powerOffset); 66void increaseSpeedRight(int powerOffset); 67void decreaseSpeedLeft(int powerOffset); 68void decreaseSpeedRight(int powerOffset); 69void rightMotor(int direction); 70void leftMotor(int direction); 71void drive(int leftDirection, int rightDirection); 72void forward(); 73void backwards(); 74void stop(); 75 76//**************************************************************************** 77// HIGH LEVEL FUNCTION 78//**************************************************************************** 79 80void initialize(int min_speed_left, int min_speed_right){ 81 pinMode(RIGHT1,OUTPUT); 82 pinMode(RIGHT2,OUTPUT); 83 pinMode(SPEEDPINRIGHT,OUTPUT); 84 pinMode(LEFT1,OUTPUT); 85 pinMode(LEFT2,OUTPUT); 86 pinMode(SPEEDPINLEFT,OUTPUT); 87 pinMode(FINDER1,INPUT); 88 pinMode(FINDER2,INPUT); 89 pinMode(FINDER3,INPUT); 90 pinMode(FINDER4,INPUT); 91 pinMode(FINDER5,INPUT); 92 directionMotorRight = directionMotorLeft = STOP; 93 initValueMotorLeft = speedmotorLeft = min_speed_left; 94 initValueMotorRight = speedmotorRight = min_speed_right; 95} 96 97int getSpeed() { 98 if (speedmotorLeft != speedmotorRight) 99 return -1; 100 return speedmotorLeft; 101} 102 103void goLeft(){ 104 if (speedmotorRight< MINSPEED) speedmotorRight = initValueMotorRight; 105 drive(STOP, FORWARD); 106} 107 108void goRight(){ 109 if (speedmotorLeft < MINSPEED) speedmotorLeft = initValueMotorLeft; 110 drive(FORWARD, STOP); 111} 112 113void goForward() { 114 if (speedmotorLeft < MINSPEED) speedmotorLeft = initValueMotorLeft; 115 if (speedmotorRight< MINSPEED) speedmotorRight = initValueMotorRight; 116 drive(FORWARD, FORWARD); 117} 118 119void goBackwards() { 120 drive (BACKWARDS, BACKWARDS); 121} 122 123void stop() { 124 drive (STOP, STOP); 125} 126 127void decreaseSpeed(int powerOffset){ 128 decreaseSpeedLeft(powerOffset); 129 decreaseSpeedRight(powerOffset); 130 drive(directionMotorLeft, directionMotorRight); 131} 132 133void increaseSpeed(int powerOffset){ 134 increaseSpeedLeft(powerOffset); 135 increaseSpeedRight(powerOffset); 136 drive(directionMotorLeft, directionMotorRight); 137} 138int readS1(){ 139 return digitalRead(FINDER1); 140} 141 142int readS2(){ 143 return digitalRead(FINDER2); 144} 145 146int readS3(){ 147 return digitalRead(FINDER3); 148} 149 150int readS4(){ 151 return digitalRead(FINDER4); 152} 153 154int readS5(){ 155 return digitalRead(FINDER5); 156} 157 158 159 160//**************************************************************************** 161// LOW LEVEL FUNCTION 162//**************************************************************************** 163 164 165void decreaseSpeedLeft(int powerOffset) { 166 powerOffset %= MAXSPEED + 1; 167 168 if (speedmotorLeft - powerOffset > initValueMotorLeft) { 169 speedmotorLeft -= powerOffset; 170 } else { 171 speedmotorLeft = initValueMotorLeft; 172 } 173} 174 175void decreaseSpeedRight(int powerOffset) { 176 powerOffset %= MAXSPEED + 1; 177 178 if (speedmotorRight - powerOffset > initValueMotorRight) { 179 speedmotorRight -= powerOffset; 180 } else { 181 speedmotorRight = initValueMotorRight; 182 } 183} 184 185void increaseSpeedLeft(int powerOffset) { 186 powerOffset %= 256; 187 188 if (powerOffset + speedmotorLeft < MAXSPEED) { 189 speedmotorLeft += powerOffset; 190 } else speedmotorLeft = 260; //Trust me, is not an error. This makes some calculations easier 191} 192 193void increaseSpeedRight(int powerOffset) { 194 powerOffset %= 256; 195 if (powerOffset + speedmotorRight < MAXSPEED) { 196 speedmotorRight += powerOffset; 197 } else speedmotorRight = 260; 198} 199 200 201void drive(int left, int right) { 202 directionMotorLeft = left; 203 directionMotorRight = right; 204 rightMotor(right); 205 leftMotor(left); 206} 207 208/** 209 * Value: 210 * if FORWARD ==> go forward 211 * if STOP ==> stop engine 212 * if BACKWARDS ==> go backwards 213 */ 214void rightMotor(int direction) { 215 if (direction == FORWARD) { 216 analogWrite(SPEEDPINRIGHT, speedmotorRight >= MAXSPEED ? 255 : speedmotorRight); 217 digitalWrite(RIGHT1, HIGH); 218 digitalWrite(RIGHT2,LOW); 219 } else if (direction == STOP) { 220 analogWrite(SPEEDPINRIGHT, 0); 221 } else { 222 analogWrite(SPEEDPINRIGHT, speedmotorRight >= MAXSPEED ? 255 : speedmotorRight); 223 digitalWrite(RIGHT1,LOW); 224 digitalWrite(RIGHT2,HIGH); 225 } 226} 227 228/** 229 * Value: 230 * if FORWARD ==> go forward 231 * if STOP ==> stop engine 232 * if BACKWARDS ==> go backwards 233 */ 234void leftMotor(int direction){ 235 if(direction == FORWARD) { 236 analogWrite(SPEEDPINLEFT, speedmotorLeft >= MAXSPEED ? 255 : speedmotorLeft); 237 digitalWrite(LEFT1,HIGH); 238 digitalWrite(LEFT2,LOW); 239 } else if(direction == STOP) { 240 analogWrite(SPEEDPINLEFT, 0); 241 } else { 242 analogWrite(SPEEDPINLEFT, speedmotorLeft >= MAXSPEED ? 255 : speedmotorLeft); 243 digitalWrite(LEFT1,LOW); 244 digitalWrite(LEFT2,HIGH); 245 } 246} 247#undef print 248#undef println 249#undef PRINTDEBUG 250#endif 251
Comments
Only logged in users can leave comments
stefano_maugeri
0 Followers
•0 Projects
Table of contents
Intro
0
0