Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.cpp@0:a7fb03c9ea9d, 2017-07-26 (annotated)

Committer:
garphil
Date:
Wed Jul 26 15:37:58 2017 +0000
Revision:
0:a7fb03c9ea9d
Child:
1:974d020bb582
First Version, missing ARC and some exhaustive test...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:a7fb03c9ea9d 1 #include "CreaBot.h"
garphil 0:a7fb03c9ea9d 2
garphil 0:a7fb03c9ea9d 3 extern Serial pc_uart;
garphil 0:a7fb03c9ea9d 4 Creabot::Creabot(Motor *left, Motor *right, float diameter_wheel_cm, float distance_wheel_cm):motor_left(left), motor_right(right), diameter_wheel(diameter_wheel_cm), distance_wheel(distance_wheel_cm)
garphil 0:a7fb03c9ea9d 5 {
garphil 0:a7fb03c9ea9d 6 perimeter_wheel = PI*diameter_wheel;
garphil 0:a7fb03c9ea9d 7 pc_uart.printf("PI %f %f %f\n\r",PI, diameter_wheel, perimeter_wheel);
garphil 0:a7fb03c9ea9d 8 perimeter_bot = PI*distance_wheel;
garphil 0:a7fb03c9ea9d 9 degree_wheel_per_cm=360.0f/perimeter_wheel;
garphil 0:a7fb03c9ea9d 10 degree_bot_per_cm=360.0f/perimeter_bot;;
garphil 0:a7fb03c9ea9d 11 ratio_wheel_bot=degree_wheel_per_cm/degree_bot_per_cm;
garphil 0:a7fb03c9ea9d 12 status=0;
garphil 0:a7fb03c9ea9d 13
garphil 0:a7fb03c9ea9d 14 motor_left->setMotorCallback(this, &Creabot::callBackLeft);
garphil 0:a7fb03c9ea9d 15 motor_right->setMotorCallback(this, &Creabot::callBackRight);
garphil 0:a7fb03c9ea9d 16 extCallBack = NULL;
garphil 0:a7fb03c9ea9d 17 setSpeed(DEFAULT_SPEED);
garphil 0:a7fb03c9ea9d 18 callLeft = true;
garphil 0:a7fb03c9ea9d 19 callRight = true;
garphil 0:a7fb03c9ea9d 20 endMove=true;
garphil 0:a7fb03c9ea9d 21 executingFifo=false;
garphil 0:a7fb03c9ea9d 22 }
garphil 0:a7fb03c9ea9d 23
garphil 0:a7fb03c9ea9d 24 void Creabot::moveForward(float cm)
garphil 0:a7fb03c9ea9d 25 {
garphil 0:a7fb03c9ea9d 26 float angle;
garphil 0:a7fb03c9ea9d 27 if(cm<0) moveBackward(cm);
garphil 0:a7fb03c9ea9d 28 angle = cm*degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 29 setSpeedLeft(current_speed);
garphil 0:a7fb03c9ea9d 30 setSpeedRight(current_speed);
garphil 0:a7fb03c9ea9d 31 moveMotorLeft(COUNTERCLOCKWISE, angle);
garphil 0:a7fb03c9ea9d 32 moveMotorRight(CLOCKWISE, angle);
garphil 0:a7fb03c9ea9d 33
garphil 0:a7fb03c9ea9d 34 }
garphil 0:a7fb03c9ea9d 35
garphil 0:a7fb03c9ea9d 36 void Creabot::moveBackward(float cm)
garphil 0:a7fb03c9ea9d 37 {
garphil 0:a7fb03c9ea9d 38 float angle;
garphil 0:a7fb03c9ea9d 39 if(cm<0) moveForward(cm);
garphil 0:a7fb03c9ea9d 40 angle = cm*degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 41 setSpeedLeft(current_speed);
garphil 0:a7fb03c9ea9d 42 setSpeedRight(current_speed);
garphil 0:a7fb03c9ea9d 43 moveMotorLeft(CLOCKWISE, angle);
garphil 0:a7fb03c9ea9d 44 moveMotorRight(COUNTERCLOCKWISE, angle);
garphil 0:a7fb03c9ea9d 45
garphil 0:a7fb03c9ea9d 46 }
garphil 0:a7fb03c9ea9d 47 void Creabot::rotate(float angleBot)
garphil 0:a7fb03c9ea9d 48 {
garphil 0:a7fb03c9ea9d 49 float angleWheel;
garphil 0:a7fb03c9ea9d 50 setSpeedLeft(current_speed);
garphil 0:a7fb03c9ea9d 51 setSpeedRight(current_speed);
garphil 0:a7fb03c9ea9d 52 angleWheel = angleBot*ratio_wheel_bot;
garphil 0:a7fb03c9ea9d 53 if(angleBot>0) {
garphil 0:a7fb03c9ea9d 54 moveMotorLeft(CLOCKWISE, angleWheel);
garphil 0:a7fb03c9ea9d 55 moveMotorRight(CLOCKWISE, angleWheel);
garphil 0:a7fb03c9ea9d 56 } else {
garphil 0:a7fb03c9ea9d 57 moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
garphil 0:a7fb03c9ea9d 58 moveMotorRight(COUNTERCLOCKWISE, angleWheel);
garphil 0:a7fb03c9ea9d 59 }
garphil 0:a7fb03c9ea9d 60 }
garphil 0:a7fb03c9ea9d 61 void Creabot::moveRight(float angle)
garphil 0:a7fb03c9ea9d 62 {
garphil 0:a7fb03c9ea9d 63 moveRight(angle,distance_wheel);
garphil 0:a7fb03c9ea9d 64 }
garphil 0:a7fb03c9ea9d 65 void Creabot::moveRight(float angle, float center_cm)
garphil 0:a7fb03c9ea9d 66 {
garphil 0:a7fb03c9ea9d 67 if(center_cm<0) center_cm=0; /* TO remove? */
garphil 0:a7fb03c9ea9d 68 float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
garphil 0:a7fb03c9ea9d 69 float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
garphil 0:a7fb03c9ea9d 70 float angleLeft = perimeter_outer*degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 71 float angleRight = perimeter_inner*degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 72 float ratio = angleRight/angleLeft;
garphil 0:a7fb03c9ea9d 73 float speedRight = current_speed * ratio;
garphil 0:a7fb03c9ea9d 74 setSpeedLeft(current_speed);
garphil 0:a7fb03c9ea9d 75 setSpeedRight(speedRight);
garphil 0:a7fb03c9ea9d 76 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
garphil 0:a7fb03c9ea9d 77 moveMotorRight(CLOCKWISE, angleRight);
garphil 0:a7fb03c9ea9d 78
garphil 0:a7fb03c9ea9d 79 }
garphil 0:a7fb03c9ea9d 80 void Creabot::moveLeft(float angle)
garphil 0:a7fb03c9ea9d 81 {
garphil 0:a7fb03c9ea9d 82 moveLeft(angle,0);
garphil 0:a7fb03c9ea9d 83 }
garphil 0:a7fb03c9ea9d 84 void Creabot::moveLeft(float angle, float center_cm)
garphil 0:a7fb03c9ea9d 85 {
garphil 0:a7fb03c9ea9d 86 if(center_cm<0) center_cm=0; /* TO remove? */
garphil 0:a7fb03c9ea9d 87 float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
garphil 0:a7fb03c9ea9d 88 float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
garphil 0:a7fb03c9ea9d 89 float angleLeft = perimeter_inner*degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 90 float angleRight = perimeter_outer*degree_wheel_per_cm;
garphil 0:a7fb03c9ea9d 91 float ratio = angleLeft/angleRight;
garphil 0:a7fb03c9ea9d 92 float speedRLeft = current_speed * ratio;
garphil 0:a7fb03c9ea9d 93 setSpeedLeft(speedRLeft);
garphil 0:a7fb03c9ea9d 94 setSpeedRight(current_speed);
garphil 0:a7fb03c9ea9d 95 moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
garphil 0:a7fb03c9ea9d 96 moveMotorRight(CLOCKWISE, angleRight);
garphil 0:a7fb03c9ea9d 97
garphil 0:a7fb03c9ea9d 98 }
garphil 0:a7fb03c9ea9d 99 void Creabot::stopMove()
garphil 0:a7fb03c9ea9d 100 {
garphil 0:a7fb03c9ea9d 101 motor_left->Stop();
garphil 0:a7fb03c9ea9d 102 motor_right->Stop();
garphil 0:a7fb03c9ea9d 103 callLeft = true;
garphil 0:a7fb03c9ea9d 104 callRight = true;
garphil 0:a7fb03c9ea9d 105 endMove = true;
garphil 0:a7fb03c9ea9d 106 flushFifo();
garphil 0:a7fb03c9ea9d 107 // callBack(); // ? To be called or set as parameter?
garphil 0:a7fb03c9ea9d 108 }
garphil 0:a7fb03c9ea9d 109 int Creabot::getStatus()
garphil 0:a7fb03c9ea9d 110 {
garphil 0:a7fb03c9ea9d 111 if(endMove == true) return 0; // No Movement
garphil 0:a7fb03c9ea9d 112 if(callRight == true) return 1; // Left Motor still running
garphil 0:a7fb03c9ea9d 113 if(callLeft == true) return 3; // ERROR
garphil 0:a7fb03c9ea9d 114 return 2; // Left Motor still running
garphil 0:a7fb03c9ea9d 115 }
garphil 0:a7fb03c9ea9d 116
garphil 0:a7fb03c9ea9d 117 void Creabot::setSpeed(float cm_per_second)
garphil 0:a7fb03c9ea9d 118 {
garphil 0:a7fb03c9ea9d 119 setSpeedLeft(cm_per_second);
garphil 0:a7fb03c9ea9d 120 setSpeedRight(cm_per_second);
garphil 0:a7fb03c9ea9d 121 current_speed = cm_per_second;
garphil 0:a7fb03c9ea9d 122 }
garphil 0:a7fb03c9ea9d 123
garphil 0:a7fb03c9ea9d 124 uint32_t Creabot::computeSpeed(Motor *motor, float cm_per_second)
garphil 0:a7fb03c9ea9d 125 {
garphil 0:a7fb03c9ea9d 126 uint32_t tick, timeUnit;
garphil 0:a7fb03c9ea9d 127 tick = motor->getCalibration();
garphil 0:a7fb03c9ea9d 128 // tick=4096;
garphil 0:a7fb03c9ea9d 129 if(cm_per_second <0.0001f) timeUnit = 1000000; else
garphil 0:a7fb03c9ea9d 130 timeUnit = perimeter_wheel * 1000000 / cm_per_second / tick;
garphil 0:a7fb03c9ea9d 131 pc_uart.printf("SET SPEED : %f %d %d %f\n\r",cm_per_second,tick,timeUnit,perimeter_wheel);
garphil 0:a7fb03c9ea9d 132 return timeUnit;
garphil 0:a7fb03c9ea9d 133 }
garphil 0:a7fb03c9ea9d 134
garphil 0:a7fb03c9ea9d 135 void Creabot::setSpeedLeft(float cm_per_second)
garphil 0:a7fb03c9ea9d 136 {
garphil 0:a7fb03c9ea9d 137 motor_left->setSpeed(computeSpeed(motor_left, cm_per_second));
garphil 0:a7fb03c9ea9d 138 }
garphil 0:a7fb03c9ea9d 139
garphil 0:a7fb03c9ea9d 140 void Creabot::setSpeedRight(float cm_per_second)
garphil 0:a7fb03c9ea9d 141 {
garphil 0:a7fb03c9ea9d 142 motor_right->setSpeed(computeSpeed(motor_right, cm_per_second));
garphil 0:a7fb03c9ea9d 143 }
garphil 0:a7fb03c9ea9d 144
garphil 0:a7fb03c9ea9d 145 void Creabot::waitEndMove()
garphil 0:a7fb03c9ea9d 146 {
garphil 0:a7fb03c9ea9d 147 pc_uart.printf("Wait end Move\n\r");
garphil 0:a7fb03c9ea9d 148 while(!endMove || executingFifo) {
garphil 0:a7fb03c9ea9d 149 wait(0.1);
garphil 0:a7fb03c9ea9d 150 }
garphil 0:a7fb03c9ea9d 151 pc_uart.printf("End Move %d %d \n\r", endMove, executingFifo);
garphil 0:a7fb03c9ea9d 152
garphil 0:a7fb03c9ea9d 153 }
garphil 0:a7fb03c9ea9d 154 void Creabot::waitEndMove(uint32_t delay_us)
garphil 0:a7fb03c9ea9d 155 {
garphil 0:a7fb03c9ea9d 156 pc_uart.printf("Wait end Move\n\r");
garphil 0:a7fb03c9ea9d 157 uint32_t v=0;
garphil 0:a7fb03c9ea9d 158 while(!endMove || executingFifo) {
garphil 0:a7fb03c9ea9d 159 wait(0.1);
garphil 0:a7fb03c9ea9d 160 v++;
garphil 0:a7fb03c9ea9d 161 if((v*10000)>delay_us) endMove = true;
garphil 0:a7fb03c9ea9d 162 }
garphil 0:a7fb03c9ea9d 163 pc_uart.printf("End Move %d %d \n\r", endMove, executingFifo);
garphil 0:a7fb03c9ea9d 164 } // watchdog
garphil 0:a7fb03c9ea9d 165
garphil 0:a7fb03c9ea9d 166 void Creabot::setCallBack(void (*callback)(int status))
garphil 0:a7fb03c9ea9d 167 {
garphil 0:a7fb03c9ea9d 168 extCallBack = callback;
garphil 0:a7fb03c9ea9d 169 }
garphil 0:a7fb03c9ea9d 170
garphil 0:a7fb03c9ea9d 171 void Creabot::fifo(cmdbot_t moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 172 {
garphil 0:a7fb03c9ea9d 173 CommandBot *cmd = fifoBot.put();
garphil 0:a7fb03c9ea9d 174 if(cmd!=NULL) {
garphil 0:a7fb03c9ea9d 175 cmd->add_command(moveType, angle_or_cm, 0);
garphil 0:a7fb03c9ea9d 176 pc_uart.printf("ADD COMMAND %d\n\r",moveType);
garphil 0:a7fb03c9ea9d 177 }
garphil 0:a7fb03c9ea9d 178 executeFifo();
garphil 0:a7fb03c9ea9d 179 }
garphil 0:a7fb03c9ea9d 180 void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
garphil 0:a7fb03c9ea9d 181 {
garphil 0:a7fb03c9ea9d 182 CommandBot *cmd = fifoBot.put();
garphil 0:a7fb03c9ea9d 183 if(cmd!=NULL) {
garphil 0:a7fb03c9ea9d 184 cmd->add_command(moveType, angle, cm);
garphil 0:a7fb03c9ea9d 185 pc_uart.printf("ADD COMMAND %d\n\r",moveType);
garphil 0:a7fb03c9ea9d 186 }
garphil 0:a7fb03c9ea9d 187 executeFifo();
garphil 0:a7fb03c9ea9d 188 }
garphil 0:a7fb03c9ea9d 189
garphil 0:a7fb03c9ea9d 190
garphil 0:a7fb03c9ea9d 191 void Creabot::moveAndWait(cmdbot_t moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 192 {
garphil 0:a7fb03c9ea9d 193 move(moveType,angle_or_cm);
garphil 0:a7fb03c9ea9d 194 waitEndMove();
garphil 0:a7fb03c9ea9d 195 }
garphil 0:a7fb03c9ea9d 196 void Creabot::moveAndWait(cmdbot_t moveType, float angle, float cm)
garphil 0:a7fb03c9ea9d 197 {
garphil 0:a7fb03c9ea9d 198 move(moveType,angle,cm);
garphil 0:a7fb03c9ea9d 199 waitEndMove();
garphil 0:a7fb03c9ea9d 200 }
garphil 0:a7fb03c9ea9d 201
garphil 0:a7fb03c9ea9d 202 void Creabot::move(cmdbot_t moveType, float angle_or_cm)
garphil 0:a7fb03c9ea9d 203 {
garphil 0:a7fb03c9ea9d 204 current_cmd.add_command(moveType, angle_or_cm, 0);
garphil 0:a7fb03c9ea9d 205 executeCommand(&current_cmd);
garphil 0:a7fb03c9ea9d 206 }
garphil 0:a7fb03c9ea9d 207 void Creabot::move(cmdbot_t moveType, float angle, float cm)
garphil 0:a7fb03c9ea9d 208 {
garphil 0:a7fb03c9ea9d 209 current_cmd.add_command(moveType, angle, cm);
garphil 0:a7fb03c9ea9d 210 executeCommand(&current_cmd);
garphil 0:a7fb03c9ea9d 211 }
garphil 0:a7fb03c9ea9d 212
garphil 0:a7fb03c9ea9d 213 void Creabot::flushFifo()
garphil 0:a7fb03c9ea9d 214 {
garphil 0:a7fb03c9ea9d 215 fifoBot.empty();
garphil 0:a7fb03c9ea9d 216 if(executingFifo) botTicker.detach();
garphil 0:a7fb03c9ea9d 217 executingFifo=false;
garphil 0:a7fb03c9ea9d 218 }
garphil 0:a7fb03c9ea9d 219 int Creabot::moveInFifo()
garphil 0:a7fb03c9ea9d 220 {
garphil 0:a7fb03c9ea9d 221 return fifoBot.getSize();
garphil 0:a7fb03c9ea9d 222 }
garphil 0:a7fb03c9ea9d 223 void Creabot::executeCommand(CommandBot *cmd) {
garphil 0:a7fb03c9ea9d 224 pc_uart.printf("EXECUTE COMMAND %d %f %f\n\r",cmd->command, cmd->angle, cmd->cm);
garphil 0:a7fb03c9ea9d 225
garphil 0:a7fb03c9ea9d 226 switch (cmd->command) {
garphil 0:a7fb03c9ea9d 227 case FORWARD:
garphil 0:a7fb03c9ea9d 228 moveForward(cmd->cm);
garphil 0:a7fb03c9ea9d 229 break;
garphil 0:a7fb03c9ea9d 230 case BACKWARD:
garphil 0:a7fb03c9ea9d 231 moveBackward(cmd->cm);
garphil 0:a7fb03c9ea9d 232 break;
garphil 0:a7fb03c9ea9d 233 case ROTATE:
garphil 0:a7fb03c9ea9d 234 rotate(cmd->angle);
garphil 0:a7fb03c9ea9d 235 break;
garphil 0:a7fb03c9ea9d 236 case LEFT:
garphil 0:a7fb03c9ea9d 237 moveLeft(cmd->angle, cmd->cm);
garphil 0:a7fb03c9ea9d 238 break;
garphil 0:a7fb03c9ea9d 239 case RIGHT:
garphil 0:a7fb03c9ea9d 240 moveRight(cmd->angle, cmd->cm);
garphil 0:a7fb03c9ea9d 241 break;
garphil 0:a7fb03c9ea9d 242 case IDLE:
garphil 0:a7fb03c9ea9d 243 // wait(10); // Not to be done in an interrupt
garphil 0:a7fb03c9ea9d 244 break;
garphil 0:a7fb03c9ea9d 245 default:
garphil 0:a7fb03c9ea9d 246 break;
garphil 0:a7fb03c9ea9d 247 };
garphil 0:a7fb03c9ea9d 248 }
garphil 0:a7fb03c9ea9d 249 void Creabot::executeFifo()
garphil 0:a7fb03c9ea9d 250 {
garphil 0:a7fb03c9ea9d 251
garphil 0:a7fb03c9ea9d 252 if(fifoBot.getSize()==0) flushFifo();
garphil 0:a7fb03c9ea9d 253 else {
garphil 0:a7fb03c9ea9d 254 if(endMove) {
garphil 0:a7fb03c9ea9d 255 CommandBot *cmd = fifoBot.get();
garphil 0:a7fb03c9ea9d 256 pc_uart.printf("Execute FIFO %d %d %d %d cmd\n\r",fifoBot.getSize(),executingFifo,endMove,cmd->command);
garphil 0:a7fb03c9ea9d 257 executeCommand(cmd);
garphil 0:a7fb03c9ea9d 258 if(!executingFifo) botTicker.attach_us(callback(this,&Creabot::executeFifo),3150);
garphil 0:a7fb03c9ea9d 259 executingFifo=true;
garphil 0:a7fb03c9ea9d 260
garphil 0:a7fb03c9ea9d 261 }
garphil 0:a7fb03c9ea9d 262 }
garphil 0:a7fb03c9ea9d 263 }
garphil 0:a7fb03c9ea9d 264
garphil 0:a7fb03c9ea9d 265 void Creabot::callBackLeft()
garphil 0:a7fb03c9ea9d 266 {
garphil 0:a7fb03c9ea9d 267 callLeft=true;
garphil 0:a7fb03c9ea9d 268 pc_uart.printf("CALL LEFT\n\r");
garphil 0:a7fb03c9ea9d 269 if(callRight)callBack();
garphil 0:a7fb03c9ea9d 270 }
garphil 0:a7fb03c9ea9d 271 void Creabot::callBackRight()
garphil 0:a7fb03c9ea9d 272 { pc_uart.printf("CALL RIGHT\n\r");
garphil 0:a7fb03c9ea9d 273
garphil 0:a7fb03c9ea9d 274 callRight=true;
garphil 0:a7fb03c9ea9d 275 if(callLeft)callBack();
garphil 0:a7fb03c9ea9d 276 }
garphil 0:a7fb03c9ea9d 277
garphil 0:a7fb03c9ea9d 278 void Creabot::callBack()
garphil 0:a7fb03c9ea9d 279 { pc_uart.printf("CALL BACK\n\r");
garphil 0:a7fb03c9ea9d 280
garphil 0:a7fb03c9ea9d 281 endMove=true;
garphil 0:a7fb03c9ea9d 282 if(extCallBack!=NULL) extCallBack(status);
garphil 0:a7fb03c9ea9d 283 }
garphil 0:a7fb03c9ea9d 284
garphil 0:a7fb03c9ea9d 285 void Creabot::moveMotorLeft(MotorDir dir, float angle)
garphil 0:a7fb03c9ea9d 286 {
garphil 0:a7fb03c9ea9d 287 if(angle>0) {
garphil 0:a7fb03c9ea9d 288 motor_left->RunDegrees(dir, angle);
garphil 0:a7fb03c9ea9d 289 endMove = callLeft = false;
garphil 0:a7fb03c9ea9d 290 }
garphil 0:a7fb03c9ea9d 291 }
garphil 0:a7fb03c9ea9d 292 void Creabot::moveMotorRight(MotorDir dir, float angle)
garphil 0:a7fb03c9ea9d 293 {
garphil 0:a7fb03c9ea9d 294 if(angle>0) {
garphil 0:a7fb03c9ea9d 295 motor_right->RunDegrees(dir, angle);
garphil 0:a7fb03c9ea9d 296 endMove = callRight = false;
garphil 0:a7fb03c9ea9d 297 }
garphil 0:a7fb03c9ea9d 298 }