Diff: CreaBot.cpp

Revision:

5:efe80c5db389

Parent:

4:531b1120d3ec

Child:

6:4d8938b686a6

--- a/CreaBot.cpp	Thu Aug 31 08:15:00 2017 +0000
+++ b/CreaBot.cpp	Wed Oct 31 14:36:07 2018 +0000
@@ -1,6 +1,52 @@
 #include "CreaBot.h"
 
-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)
+// *****************************************************************
+//    Motor Command and Command-FIFO classes
+// *****************************************************************
+
+void MotCommand::assign(cmdbot_t cmd, float p1, float p2) 
+{   command = cmd; 
+    switch (command) {
+        case FORWARD : ; 
+        case BACKWARD: cm=p1; angle=0; break; 
+        case ROTATE  : angle = p1; break; 
+        case LEFT    : 
+        case RIGHT   : angle = p1; cm=p2; break; 
+        case IDLE    : break; 
+        default      : break;
+    };
+}; 
+
+MotCommand *CommandFIFO::put() {
+    if(count<DEPTH_FIFO) {
+        int old_writeIdx = writeIdx;
+        writeIdx++;
+        if (writeIdx==DEPTH_FIFO) writeIdx=0; 
+        count++;
+        return &cmd[old_writeIdx];
+        } 
+      else 
+        return NULL;
+}; // put()
+
+MotCommand *CommandFIFO::get() {
+    if ( !isEmpty() ) {
+            int old_readIdx = readIdx; 
+        readIdx++;
+        if (readIdx==DEPTH_FIFO) readIdx=0; 
+        count--; 
+        return &cmd[old_readIdx];
+        } 
+      else 
+        return NULL;
+}; // get()
+
+// *****************************************************************
+//    Creabot Class
+// *****************************************************************
+
+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;
     perimeter_bot = PI*distance_wheel;
@@ -9,45 +55,41 @@
     ratio_wheel_bot=perimeter_bot/perimeter_wheel;
     status=0;
 
- motor_left->setMotorCallback(this, &Creabot::callBackLeft);
- motor_right->setMotorCallback(this, &Creabot::callBackRight);
+    motor_left->setMotorCallback(this, &Creabot::callBackLeft);
+    motor_right->setMotorCallback(this, &Creabot::callBackRight);
     extCallBack = NULL;
     setSpeed(DEFAULT_SPEED);
-    callLeft = true;
-    callRight = true;
+    endMoveLeft = true;
+    endMoveRight = 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);
-
+    if (cm<0) moveBackward(cm);
+    else {
+        float angle = cm*degree_wheel_per_cm;
+        setSpeed(last_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);
-
+    else {
+        float angle = cm*degree_wheel_per_cm;
+        setSpeed(last_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;
+    float angleWheel = angleBot*ratio_wheel_bot;
+    setSpeed(last_speed);
     if(angleBot<0) {
         moveMotorLeft(CLOCKWISE, -angleWheel);
         moveMotorRight(CLOCKWISE, -angleWheel);
@@ -56,88 +98,17 @@
         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;
-   // 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);
-    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;
+    motor_left->MotorOFF();
+    motor_right->MotorOFF();
+    endMoveLeft = true;
+    endMoveRight = 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();
-  if(cm_per_second <0.0001f) timeUnit = 1000000; else 
-    timeUnit = perimeter_wheel * 1000000 / cm_per_second / tick;
-    return timeUnit;
-}
-
-void Creabot::setSpeedLeft(float cm_per_second)
-{
-    motor_left->setDelayBtwTicks(computeSpeed(motor_left, cm_per_second));
-}
-
-void Creabot::setSpeedRight(float cm_per_second)
-{
-    motor_right->setDelayBtwTicks(computeSpeed(motor_right, cm_per_second));
-}
 
 void Creabot::waitEndMove()
 {
@@ -163,42 +134,32 @@
 
 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);
-    }
-    executeFifo();
-}
-void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
-{
-    CommandBot *cmd = fifoBot.put();
-    if(cmd!=NULL) {
-        cmd->add_command(moveType, angle, cm);
-   }
+    MotCommand *cmd = fifoBot.put();
+    if(cmd != NULL) {
+        cmd->assign(moveType, angle_or_cm, 0);
+        }
     executeFifo();
 }
 
+void Creabot::fifo(cmdbot_t moveType, float angle, float cm)
+{
+    MotCommand *cmd = fifoBot.put();
+    if(cmd!=NULL) {
+        cmd->assign(moveType, angle, cm);
+        }
+    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();
+    move(moveType,angle_or_cm);
+    waitEndMove();
 }
 
-void Creabot::move(cmdbot_t moveType, float angle_or_cm)
+void Creabot::moveAndWait(cmdbot_t moveType, float angle, float 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);
+    move(moveType,angle,cm);
+    waitEndMove();
 }
 
 void Creabot::flushFifo()
@@ -207,11 +168,71 @@
     if(executingFifo) botTicker.detach();
     executingFifo=false;
 }
+
 int Creabot::moveInFifo()
 {
-    return fifoBot.getSize();
+    return fifoBot.getCount();
+}
+
+void Creabot::spirale(float b, float turns) {
+    float r=0.0;
+    float angle = 0.0f;
+    while(angle < float( 360.0 * turns) ) {
+        this->moveAndWait(RIGHT, 10, r);
+        r=r+b;
+    }
+}
+
+void Creabot::callBackLeft()
+{
+    endMoveLeft=true;
+    if(endMoveRight)callBack();
+}
+
+void Creabot::callBackRight()
+{
+    endMoveRight=true;
+    if(endMoveLeft)callBack();
+}
+
+void Creabot::callBack()
+{
+    endMove=true;
+    if(extCallBack!=NULL) extCallBack(status);
 }
-void Creabot::executeCommand(CommandBot *cmd) {
+
+
+//******************************************
+//    Executing the FIFO of Motor Commands 
+//******************************************
+
+// This is the own callback function of the ticker, that works throught the FIFO stack
+void Creabot::executeFifo()
+{   if (fifoBot.isEmpty()) flushFifo();
+    else {
+        if(endMove) {
+            MotCommand *cmd = fifoBot.get();
+            executeCommand(cmd);
+            if (!executingFifo) botTicker.attach_us( callback( this, &Creabot::executeFifo), 3150);
+            executingFifo=true;
+        }
+    }
+}
+
+void Creabot::move(cmdbot_t moveType, float angle_or_cm)
+{
+    current_cmd.assign(moveType, angle_or_cm, 0);
+    executeCommand(&current_cmd);
+}
+
+void Creabot::move(cmdbot_t moveType, float angle, float cm)
+{
+    current_cmd.assign(moveType, angle, cm);
+    executeCommand(&current_cmd);
+}
+
+// Direct Command Execution, called by FIFO-Handler. 
+void Creabot::executeCommand(MotCommand *cmd) {
           switch (cmd->command) {
             case FORWARD:
                 moveForward(cmd->cm);
@@ -234,59 +255,91 @@
             default:
                 break;
         };
-}
-void Creabot::executeFifo()
-{
+}; // executeCommand
+
 
-    if(fifoBot.getSize()==0) flushFifo();
-    else {
-        if(endMove) {
-            CommandBot *cmd = fifoBot.get();
-            executeCommand(cmd);
-            if(!executingFifo) botTicker.attach_us(callback(this,&Creabot::executeFifo),3150);
-            executingFifo=true;
+//***********************************************
+//    Execute high level motor functions directly, no FIFO
+//***********************************************
 
-        }
-    }
+void Creabot::moveRight(float angle)
+{
+    moveRight(angle,distance_wheel);
 }
 
-void Creabot::spirale(float b, float turns) {
-    float r=0.0;
-    float angle = 0.0f;
-    while(angle < 360.0*turns) {
-        this->moveAndWait(RIGHT, 10, r);
-        r=r+b;
-    }
+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 = last_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, last_speed);
+    setSpeedMot(motor_left,last_speed);
+    setSpeedMot(motor_right,speedRight);
+    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
+    moveMotorRight(CLOCKWISE, angleRight);
+}
+
+void Creabot::moveLeft(float angle)
+{
+    moveLeft(angle,0);
 }
 
-void Creabot::callBackLeft()
+void Creabot::moveLeft(float angle, float center_cm)
 {
-    callLeft=true;
-    if(callRight)callBack();
-}
-void Creabot::callBackRight()
-{
-    callRight=true;
-    if(callLeft)callBack();
+    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 = last_speed * ratio;
+    setSpeedMot(motor_left,speedRLeft);
+    setSpeedMot(motor_right,last_speed);
+    moveMotorLeft(COUNTERCLOCKWISE, angleLeft);
+    moveMotorRight(CLOCKWISE, angleRight);
 }
 
-void Creabot::callBack()
-{
-    endMove=true;
-    if(extCallBack!=NULL) extCallBack(status);
-}
+//******************************************************************************
+// low level motor functions, filters for angle>0, remembers that a motor moves
+//******************************************************************************
 
-void Creabot::moveMotorLeft(MotorDir dir, float angle)
+void Creabot::moveMotorLeft(motorDir dir, float angle)
 {
    if(angle>0) {
         motor_left->RunDegrees(dir, angle);
-        endMove = callLeft = false;
+        endMove = endMoveLeft = false;
    }
 }
-void Creabot::moveMotorRight(MotorDir dir, float angle)
-{
-    if(angle>0) {
+
+void Creabot::moveMotorRight(motorDir dir, float angle)
+{    if(angle>0) {
         motor_right->RunDegrees(dir, angle);
-        endMove = callRight = false;
-    }
-}
\ No newline at end of file
+        endMove = endMoveRight = false;}
+}
+
+int Creabot::getStatus()
+{   if(endMove     ) return 0; // No Movement
+    if(endMoveRight) return 1; // Returns 1, assuming that only Left Motor is still running
+    if(endMoveLeft ) return 3; // Returns 2, Right Motor is running aloone
+    return 2; // Both Motors are still running!!!
+}
+
+void Creabot::setSpeed(float cm_per_second)
+{
+    setSpeedMot(motor_left,cm_per_second);
+    setSpeedMot(motor_right,cm_per_second);
+    last_speed = cm_per_second;
+}
+
+void Creabot::setSpeedMot(Motor *motor, float cm_per_second)
+{   if (cm_per_second <0.0001f) 
+           motor->setStepTime_us( 1000000 ); 
+    else { uint32_t tick = motor->getStepsFullTurn();
+           motor->setStepTime_us( perimeter_wheel * 1000000 / cm_per_second / tick ) ;
+        }
+}
+