Merge pull request #23 from arduino-libraries/feature_position

ack on wheel position 1.0.3

Author: gbr1

examples/firmware/firmware.ino

@@ -122,6 +122,8 @@ void loop(){
               break;
             case 'P':
               alvik.setPositionLeft(value);
+              ack_required=MOVEMENT_LEFT;
+              ack_check=true;
               break;
             case 'Z':
               alvik.resetPositionLeft(value);
@@ -136,6 +138,8 @@ void loop(){
               break;
             case 'P':
               alvik.setPositionRight(value);
+              ack_required=MOVEMENT_RIGHT;
+              ack_check=true;
               break;
             case 'Z':
               alvik.resetPositionRight(value);
@@ -149,6 +153,8 @@ void loop(){
         packeter.unpacketC2F(code,position_left, position_right);
         alvik.disableKinematicsMovement();
         alvik.setPosition(position_left, position_right);
+        ack_required=MOVEMENT_POSITION;
+        ack_check=true;
         break;
 
 
@@ -280,13 +286,22 @@ void loop(){
       msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
       alvik.serial->write(packeter.msg,msg_size);
     }
-    if (ack_check && alvik.isTargetReached()){
+    if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){
       if (ack_required == MOVEMENT_ROTATE){
         msg_size = packeter.packetC1B('x', 'R');
       }
       if (ack_required == MOVEMENT_MOVE){
         msg_size = packeter.packetC1B('x', 'M');
       }
+      if (ack_required == MOVEMENT_POSITION){
+        msg_size = packeter.packetC1B('x', 'P');
+      }
+      if (ack_required == MOVEMENT_LEFT){
+        msg_size = packeter.packetC1B('x', 'P');
+      }
+      if (ack_required == MOVEMENT_RIGHT){
+        msg_size = packeter.packetC1B('x', 'P');
+      }
     }
     else{
       msg_size = packeter.packetC1B('x', 0);

library.properties

@@ -1,5 +1,5 @@
 name=Arduino_AlvikCarrier
-version=1.0.2
+version=1.0.3
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <[email protected]>
 sentence=Library and firmware for Arduino Alvik Carrier board

src/Arduino_AlvikCarrier.cpp

@@ -362,6 +362,10 @@ float Arduino_AlvikCarrier::getPositionLeft(){
     return motor_control_left->getPosition();
 }
 
+bool Arduino_AlvikCarrier::isPositionLeftReached(){
+    return motor_control_left->isPositionReached();
+}
+
 void Arduino_AlvikCarrier::setPositionRight(const float degrees){
     motor_control_right->setPosition(degrees);
 }  
@@ -370,6 +374,10 @@ float Arduino_AlvikCarrier::getPositionRight(){
     return motor_control_right->getPosition();
 }
 
+bool Arduino_AlvikCarrier::isPositionRightReached(){
+    return motor_control_right->isPositionReached();
+}
+
 void Arduino_AlvikCarrier::setPosition(const float left_deg, const float right_deg){
     setPositionLeft(left_deg);
     setPositionRight(right_deg);
@@ -380,6 +388,10 @@ void Arduino_AlvikCarrier::getPosition(float & left_deg, float & right_deg){
     right_deg = getPositionRight();
 }
 
+bool Arduino_AlvikCarrier::isPositionReached(){
+    return isPositionLeftReached() && isPositionRightReached();
+}
+
 void Arduino_AlvikCarrier::resetPositionLeft(const float initial_position){
     motor_control_left->resetPosition(initial_position);
 }

src/Arduino_AlvikCarrier.h

@@ -172,10 +172,13 @@ class Arduino_AlvikCarrier{
         void setKPidRight(const float kp, const float ki, const float kd);              // set PID parameters for right wheel
         void setPositionLeft(const float degrees);                                      // set position in degrees on left wheel
         float getPositionLeft();                                                        // get left wheel position in degrees
+        bool isPositionLeftReached();                                                   // return true if left wheel position is reached
         void setPositionRight(const float degrees);                                     // set position in degrees on right wheel
         float getPositionRight();                                                       // get right wheel position in degrees
+        bool isPositionRightReached();                                                  // return true if right wheel position is reached
         void setPosition(const float left_deg, const float right_deg);                  // set positions on both wheels
         void getPosition(float & left_deg, float & right_deg);                          // get both wheels position
+        bool isPositionReached();                                                       // return true if both wheels position are reached
         void resetPositionLeft(const float initial_position=0.0);                       // reset/set value of position for left wheel
         void resetPositionRight(const float initial_position=0.0);                      // reset/set value of position for right wheel
         void disablePositionControlLeft();                                              // disable the position control on left wheel

src/definitions/robot_definitions.h

@@ -37,6 +37,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define POSITION_KD_DEFAULT 0.0001
 #define POSITION_CONTROL_PERIOD 0.02
 #define POSITION_MAX_SPEED 30.0
+#define POSITION_THRESHOLD 10
 
 
 
@@ -58,6 +59,9 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define MOVEMENT_DISABLED 0
 #define MOVEMENT_ROTATE 1
 #define MOVEMENT_MOVE 2
+#define MOVEMENT_LEFT 3
+#define MOVEMENT_RIGHT 4
+#define MOVEMENT_POSITION 5
 
 
 #define LIFT_THRESHOLD 80
@@ -88,7 +92,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 // Library version
 #define VERSION_BYTE_HIGH 1
 #define VERSION_BYTE_MID 0
-#define VERSION_BYTE_LOW 2
+#define VERSION_BYTE_LOW 3
 
 
 

src/motor_control/motor_control.cpp

@@ -35,6 +35,7 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
     pos_controller_period = _pos_controller_period;
     pos_max_velocity = _pos_max_velocity;
     position_control_enabled = false;
+    is_position_reached = false;
 
 
 
@@ -168,6 +169,9 @@ void MotorControl::update(){
     if (position_control_enabled){
         pos_pid->update(position);
         setRPM(round(pos_pid->getControlOutput()/10.0)*10);
+        if (abs(pos_pid->getError())<POSITION_THRESHOLD){
+            is_position_reached = true;
+        }
     }
 }
 
@@ -210,6 +214,7 @@ void MotorControl::enablePositionControl(){
 
 void MotorControl::disablePositionControl(){
     position_control_enabled = false;
+    is_position_reached = false;
 }
 
 bool MotorControl::isPositionControlEnabled(){
@@ -230,5 +235,10 @@ float MotorControl::getPosition(){
 void MotorControl::setPosition(const float degree){
     pos_pid->setReference(degree);
     enablePositionControl();
+    is_position_reached = false;
+}
+
+bool MotorControl::isPositionReached(){
+    return is_position_reached;
 }
 

src/motor_control/motor_control.h

@@ -43,6 +43,7 @@ class MotorControl{
         float pos_controller_period;
         float pos_max_velocity;
         bool position_control_enabled;
+        bool is_position_reached;
 
         float position;
         float angle;
@@ -112,6 +113,7 @@ class MotorControl{
         void setPosition(const float degree);                          // set the reference for position control
         float getPosition();                                            // get the actual angle in degrees of motor
         void  resetPosition(const float p0=0.0);                        // reset/set the position value
+        bool isPositionReached();
 
         float getError();