Tarek Lule / CreaBotLib

My Version of CreaBotLib

Fork of CreaBotLib by CreaLab

CreaBot.cpp@1:974d020bb582, 2017-07-27 (annotated)

Committer:
garphil
Date:
Thu Jul 27 21:27:15 2017 +0000
Revision:
1:974d020bb582
Parent:
0:a7fb03c9ea9d
Child:
2:3d2d6d655d01
Solved issue on move(RIGHT & LEFT in motorLib + precision + removed printf.

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