Diff: CreaBot.cpp

Revision:

1:974d020bb582

Parent:

0:a7fb03c9ea9d

Child:

2:3d2d6d655d01

--- a/CreaBot.cpp	Wed Jul 26 15:37:58 2017 +0000
+++ b/CreaBot.cpp	Thu Jul 27 21:27:15 2017 +0000
@@ -1,14 +1,12 @@
 #include "CreaBot.h"
 
-extern Serial pc_uart;
 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)
 {
     perimeter_wheel = PI*diameter_wheel;
-    pc_uart.printf("PI %f %f %f\n\r",PI, diameter_wheel, perimeter_wheel);
     perimeter_bot = PI*distance_wheel;
     degree_wheel_per_cm=360.0f/perimeter_wheel;
     degree_bot_per_cm=360.0f/perimeter_bot;;
-    ratio_wheel_bot=degree_wheel_per_cm/degree_bot_per_cm;
+    ratio_wheel_bot=perimeter_wheel/perimeter_bot;
     status=0;
 
  motor_left->setMotorCallback(this, &Creabot::callBackLeft);
@@ -54,8 +52,8 @@
         moveMotorLeft(CLOCKWISE, angleWheel);
         moveMotorRight(CLOCKWISE, angleWheel);
     } else {
-        moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
-        moveMotorRight(COUNTERCLOCKWISE, angleWheel);
+        moveMotorLeft(COUNTERCLOCKWISE, -angleWheel);
+        moveMotorRight(COUNTERCLOCKWISE, -angleWheel);
     }
 }
 void Creabot::moveRight(float angle)
@@ -71,6 +69,7 @@
     float  angleRight = perimeter_inner*degree_wheel_per_cm;
     float ratio = angleRight/angleLeft;
     float speedRight = current_speed * ratio;
+   // 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);
     setSpeedLeft(current_speed);
     setSpeedRight(speedRight);
     moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
@@ -125,42 +124,36 @@
 {
     uint32_t tick, timeUnit;
     tick = motor->getCalibration();
-  //  tick=4096;
   if(cm_per_second <0.0001f) timeUnit = 1000000; else 
     timeUnit = perimeter_wheel * 1000000 / cm_per_second / tick;
-    pc_uart.printf("SET SPEED : %f %d %d %f\n\r",cm_per_second,tick,timeUnit,perimeter_wheel);
     return timeUnit;
 }
 
 void Creabot::setSpeedLeft(float cm_per_second)
 {
-    motor_left->setSpeed(computeSpeed(motor_left, cm_per_second));
+    motor_left->setDelayBtwTicks(computeSpeed(motor_left, cm_per_second));
 }
 
 void Creabot::setSpeedRight(float cm_per_second)
 {
-    motor_right->setSpeed(computeSpeed(motor_right, cm_per_second));
+    motor_right->setDelayBtwTicks(computeSpeed(motor_right, cm_per_second));
 }
 
 void Creabot::waitEndMove()
 {
- pc_uart.printf("Wait end Move\n\r");
    while(!endMove || executingFifo) {
        wait(0.1);
      }
-  pc_uart.printf("End Move %d %d \n\r", endMove, executingFifo);
-    
+   
 }
 void Creabot::waitEndMove(uint32_t delay_us)
 {
-pc_uart.printf("Wait end Move\n\r");
     uint32_t v=0;
     while(!endMove || executingFifo) {
         wait(0.1);
         v++;
         if((v*10000)>delay_us) endMove = true;
     }
-  pc_uart.printf("End Move %d %d \n\r", endMove, executingFifo);
 } // watchdog
 
 void Creabot::setCallBack(void (*callback)(int status))
@@ -173,7 +166,6 @@
     CommandBot *cmd = fifoBot.put();
     if(cmd!=NULL) {
         cmd->add_command(moveType, angle_or_cm, 0);
-        pc_uart.printf("ADD COMMAND %d\n\r",moveType);
     }
     executeFifo();
 }
@@ -182,7 +174,6 @@
     CommandBot *cmd = fifoBot.put();
     if(cmd!=NULL) {
         cmd->add_command(moveType, angle, cm);
-         pc_uart.printf("ADD COMMAND %d\n\r",moveType);
    }
     executeFifo();
 }
@@ -221,8 +212,6 @@
     return fifoBot.getSize();
 }
 void Creabot::executeCommand(CommandBot *cmd) {
-                  pc_uart.printf("EXECUTE COMMAND %d %f %f\n\r",cmd->command, cmd->angle, cmd->cm);
-
           switch (cmd->command) {
             case FORWARD:
                 moveForward(cmd->cm);
@@ -240,7 +229,7 @@
                 moveRight(cmd->angle, cmd->cm);
                 break;
             case IDLE:
-              //  wait(10); // Not to be done in an interrupt
+              //  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... 
                 break;
             default:
                 break;
@@ -253,7 +242,6 @@
     else {
         if(endMove) {
             CommandBot *cmd = fifoBot.get();
-            pc_uart.printf("Execute FIFO %d %d %d %d cmd\n\r",fifoBot.getSize(),executingFifo,endMove,cmd->command);
             executeCommand(cmd);
             if(!executingFifo) botTicker.attach_us(callback(this,&Creabot::executeFifo),3150);
             executingFifo=true;
@@ -265,19 +253,16 @@
 void Creabot::callBackLeft()
 {
     callLeft=true;
-    pc_uart.printf("CALL LEFT\n\r");
     if(callRight)callBack();
 }
 void Creabot::callBackRight()
-{    pc_uart.printf("CALL RIGHT\n\r");
-
+{
     callRight=true;
     if(callLeft)callBack();
 }
 
 void Creabot::callBack()
-{    pc_uart.printf("CALL BACK\n\r");
-
+{
     endMove=true;
     if(extCallBack!=NULL) extCallBack(status);
 }