eLab Team / Mbed 2 deprecated myRobot

Working version without LEDs

Dependencies:   mbed WS2812

Voici le dernier schéma de cablage (version du 08/02/2020)

https://os.mbed.com/media/uploads/max_ence/schemarobot_fev2020.pdf

CreabotLib/CreaBot.cpp@1:69b5d8f0ba9c, 2020-05-30 (annotated)

Committer:
elab
Date:
Sat May 30 09:31:57 2020 +0000
Revision:
1:69b5d8f0ba9c
Parent:
0:0e577ce96b2f 
pour eLab;

Who changed what in which revision?

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