Diff: CreaBot.cpp

Revision:

0:a7fb03c9ea9d

Child:

1:974d020bb582

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CreaBot.cpp	Wed Jul 26 15:37:58 2017 +0000
@@ -0,0 +1,298 @@
+#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;
+    status=0;
+
+ motor_left->setMotorCallback(this, &Creabot::callBackLeft);
+ motor_right->setMotorCallback(this, &Creabot::callBackRight);
+    extCallBack = NULL;
+    setSpeed(DEFAULT_SPEED);
+    callLeft = true;
+    callRight = true;
+    endMove=true;
+    executingFifo=false;
+}
+
+void Creabot::moveForward(float cm)
+{
+    float angle;
+    if(cm<0) moveBackward(cm);
+    angle = cm*degree_wheel_per_cm;
+    setSpeedLeft(current_speed);
+    setSpeedRight(current_speed);
+   moveMotorLeft(COUNTERCLOCKWISE, angle);
+    moveMotorRight(CLOCKWISE, angle);
+
+}
+
+void Creabot::moveBackward(float cm)
+{
+    float angle;
+    if(cm<0) moveForward(cm);
+    angle = cm*degree_wheel_per_cm;
+   setSpeedLeft(current_speed);
+    setSpeedRight(current_speed);
+    moveMotorLeft(CLOCKWISE, angle);
+    moveMotorRight(COUNTERCLOCKWISE, angle);
+
+}
+void Creabot::rotate(float angleBot)
+{
+    float angleWheel;
+    setSpeedLeft(current_speed);
+    setSpeedRight(current_speed);
+   angleWheel = angleBot*ratio_wheel_bot;
+    if(angleBot>0) {
+        moveMotorLeft(CLOCKWISE, angleWheel);
+        moveMotorRight(CLOCKWISE, angleWheel);
+    } else {
+        moveMotorLeft(COUNTERCLOCKWISE, angleWheel);
+        moveMotorRight(COUNTERCLOCKWISE, angleWheel);
+    }
+}
+void Creabot::moveRight(float angle)
+{
+    moveRight(angle,distance_wheel);
+}
+void Creabot::moveRight(float angle, float center_cm)
+{
+    if(center_cm<0) center_cm=0; /* TO remove? */
+    float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
+    float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
+    float  angleLeft = perimeter_outer*degree_wheel_per_cm;
+    float  angleRight = perimeter_inner*degree_wheel_per_cm;
+    float ratio = angleRight/angleLeft;
+    float speedRight = current_speed * ratio;
+    setSpeedLeft(current_speed);
+    setSpeedRight(speedRight);
+    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
+    moveMotorRight(CLOCKWISE, angleRight);
+
+}
+void Creabot::moveLeft(float angle)
+{
+    moveLeft(angle,0);
+}
+void Creabot::moveLeft(float angle, float center_cm)
+{
+    if(center_cm<0) center_cm=0; /* TO remove? */
+    float perimeter_outer = 2.0f*angle*(center_cm+distance_wheel)*PI/360.0f;
+    float perimeter_inner = 2.0f*angle*(center_cm)*PI/360.0f;
+    float  angleLeft = perimeter_inner*degree_wheel_per_cm;
+    float  angleRight = perimeter_outer*degree_wheel_per_cm;
+    float ratio = angleLeft/angleRight;
+    float speedRLeft = current_speed * ratio;
+    setSpeedLeft(speedRLeft);
+    setSpeedRight(current_speed);
+    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
+    moveMotorRight(CLOCKWISE, angleRight);
+
+}
+void Creabot::stopMove()
+{
+    motor_left->Stop();
+    motor_right->Stop();
+    callLeft = true;
+    callRight = true;
+    endMove = true;
+    flushFifo();
+    //  callBack(); // ? To be called or set as parameter?
+}
+int Creabot::getStatus()
+{
+    if(endMove == true) return 0; // No Movement
+    if(callRight == true) return 1; // Left Motor still running
+    if(callLeft == true) return 3; // ERROR
+    return 2; // Left Motor still running
+}
+
+void Creabot::setSpeed(float cm_per_second)
+{
+    setSpeedLeft(cm_per_second);
+    setSpeedRight(cm_per_second);
+    current_speed = cm_per_second;
+}
+
+uint32_t Creabot::computeSpeed(Motor *motor, float cm_per_second)
+{
+    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));
+}
+
+void Creabot::setSpeedRight(float cm_per_second)
+{
+    motor_right->setSpeed(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))
+{
+    extCallBack = callback;
+}
+
+void Creabot::fifo(cmdbot_t moveType, float angle_or_cm)
+{
+    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();
+}
+void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
+{
+    CommandBot *cmd = fifoBot.put();
+    if(cmd!=NULL) {
+        cmd->add_command(moveType, angle, cm);
+         pc_uart.printf("ADD COMMAND %d\n\r",moveType);
+   }
+    executeFifo();
+}
+
+
+void Creabot::moveAndWait(cmdbot_t moveType, float angle_or_cm)
+{
+        move(moveType,angle_or_cm);
+        waitEndMove();
+}
+void Creabot::moveAndWait(cmdbot_t moveType, float angle, float cm)
+{
+        move(moveType,angle,cm);
+        waitEndMove();
+}
+
+void Creabot::move(cmdbot_t moveType, float angle_or_cm)
+{
+        current_cmd.add_command(moveType, angle_or_cm, 0);
+    executeCommand(&current_cmd);
+}
+void Creabot::move(cmdbot_t moveType, float angle, float cm)
+{
+         current_cmd.add_command(moveType, angle, cm);
+     executeCommand(&current_cmd);
+}
+
+void Creabot::flushFifo()
+{
+    fifoBot.empty();
+    if(executingFifo) botTicker.detach();
+    executingFifo=false;
+}
+int Creabot::moveInFifo()
+{
+    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);
+                break;
+            case BACKWARD:
+                moveBackward(cmd->cm);
+                break;
+            case ROTATE:
+                rotate(cmd->angle);
+                break;
+            case LEFT:
+                moveLeft(cmd->angle, cmd->cm);
+                break;
+            case RIGHT:
+                moveRight(cmd->angle, cmd->cm);
+                break;
+            case IDLE:
+              //  wait(10); // Not to be done in an interrupt
+                break;
+            default:
+                break;
+        };
+}
+void Creabot::executeFifo()
+{
+
+    if(fifoBot.getSize()==0) flushFifo();
+    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;
+
+        }
+    }
+}
+
+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);
+}
+
+void Creabot::moveMotorLeft(MotorDir dir, float angle)
+{
+   if(angle>0) {
+        motor_left->RunDegrees(dir, angle);
+        endMove = callLeft = false;
+   }
+}
+void Creabot::moveMotorRight(MotorDir dir, float angle)
+{
+    if(angle>0) {
+        motor_right->RunDegrees(dir, angle);
+        endMove = callRight = false;
+    }
+}
\ No newline at end of file