Merge pull request #8 from arduino-libraries/devel

0.3.0

examples/firmware_01/firmware_01.ino → examples/firmware/firmware.ino

@@ -28,15 +28,17 @@ uint8_t code;
 uint8_t label;
 uint8_t control_type;
 uint8_t msg_size;
-uint8_t ack_required=0;
-bool ack_check=false;
-uint8_t ack_code=0;
+uint8_t ack_required = 0;
+bool ack_check = false;
+uint8_t ack_code = 0;
+uint8_t behaviours;
 
-unsigned long tmotor=0;
-unsigned long tsend=0;
-unsigned long tsensor=0;
-unsigned long timu=0;
-unsigned long tack=0;
+unsigned long tmotor = 0;
+unsigned long tsend = 0;
+unsigned long tsensor = 0;
+unsigned long timu = 0;
+unsigned long tack = 0;
+unsigned long tbehaviours = 0;
 
 
 float left, right, value;
@@ -51,6 +53,10 @@ float kp, ki, kd;
 float x, y, theta;
 
 uint8_t servo_A, servo_B;
+float position_left, position_right;
+
+int counter_version = 9;
+uint8_t version[3];
 
 
 void setup(){
@@ -62,7 +68,7 @@ void setup(){
   line.begin();
   tof.begin();
 
-  uint8_t version[3];
+
   alvik.getVersion(version[0], version[1], version[2]);
   msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
   alvik.serial->write(packeter.msg,msg_size);
@@ -77,6 +83,7 @@ void setup(){
   tsensor=millis();
   timu=millis();
   tack=millis();
+  tbehaviours=millis();
 }
 
 void loop(){
@@ -132,6 +139,14 @@ void loop(){
           }
         }
         break;
+
+
+      case 'A':
+        packeter.unpacketC2F(code,position_left, position_right);
+        alvik.disableKinematicsMovement();
+        alvik.setPosition(position_left, position_right);
+        break;
+
 
       case 'S':
         packeter.unpacketC2B(code,servo_A,servo_B);
@@ -181,6 +196,17 @@ void loop(){
           ack_check = false;
         }
         break;
+
+      case 'B':
+        packeter.unpacketC1B(code, behaviours);
+        switch (behaviours){
+          case 1: 
+            alvik.setBehaviour(LIFT_ILLUMINATOR, true);
+            break;
+          default:
+            alvik.setBehaviour(ALL_BEHAVIOURS, false);
+        }
+        break;
     }
   }
 
@@ -205,7 +231,7 @@ void loop(){
         break;
       case 3:
         if (tof.update_rois()){
-          msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.get_min_range_top_mm(), tof.get_max_range_bottom_mm());
+          msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.getTop(), tof.getBottom());
           alvik.serial->write(packeter.msg,msg_size);
         }
         break;
@@ -221,7 +247,7 @@ void loop(){
   } 
 
   // motors update & publish
-  if (millis()-tmotor>20){
+  if (millis()-tmotor>=20){
     tmotor=millis();
     alvik.updateMotors();
     alvik.updateKinematics();
@@ -242,6 +268,12 @@ void loop(){
   // acknowledge
   if (millis()-tack > 100){
     tack = millis();
+    if (counter_version>0){
+      counter_version--;
+      alvik.getVersion(version[0], version[1], version[2]);
+      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_required == MOVEMENT_ROTATE){
         msg_size = packeter.packetC1B('x', 'R');
@@ -256,6 +288,11 @@ void loop(){
     alvik.serial->write(packeter.msg, msg_size);
   }
 
+  if (millis()-tbehaviours > 100){
+    tbehaviours = millis();
+    alvik.updateBehaviours();
+  }
+
   // imu update
   if (millis()-timu>10){
     timu=millis();

library.properties

@@ -1,5 +1,5 @@
-name=Arduino Alvik Carrier
-version=0.2.0
+name=Arduino_AlvikCarrier
+version=0.3.0
 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

@@ -133,6 +133,8 @@ int Arduino_AlvikCarrier::begin(){
     if (beginImu()!=0){
         errorLed(ERROR_IMU);
     }
+
+    beginBehaviours();
 
 
     return 0;
@@ -171,15 +173,18 @@ void Arduino_AlvikCarrier::updateAPDS(){
 }
 
 void Arduino_AlvikCarrier::setIlluminator(uint8_t value){
+    illuminator_state=value;
     digitalWrite(APDS_LED, value);
 }
 
 void Arduino_AlvikCarrier::enableIlluminator(){
     setIlluminator(HIGH);
+    prev_illuminator_state = true;
 }
 
 void Arduino_AlvikCarrier::disableIlluminator(){
     setIlluminator(LOW);
+    prev_illuminator_state = false;
 }
 
 int Arduino_AlvikCarrier::getRed(){
@@ -540,7 +545,18 @@ void Arduino_AlvikCarrier::setLeds(const uint32_t red, const uint32_t green, con
 
 void Arduino_AlvikCarrier::setAllLeds(const uint8_t value){
     setLedBuiltin(value&1);
-    setIlluminator((value>>1)&1);
+    //setIlluminator((value>>1)&1);
+    if ((value>>1)&1){
+        if ((behaviours|=1 == 1)&&isLifted()){
+            prev_illuminator_state = true;
+        }
+        else{
+            enableIlluminator();
+        }
+    }
+    else{
+        disableIlluminator();
+    }
     setLedLeftRed(((value>>2)&1));
     setLedLeftGreen(((value>>3)&1));
     setLedLeftBlue(((value>>4)&1));
@@ -811,3 +827,69 @@ bool Arduino_AlvikCarrier::isTargetReached(){
 uint8_t Arduino_AlvikCarrier::getKinematicsMovement(){
     return kinematics_movement;
 }
+
+
+
+/******************************************************************************************************/
+/*                                             Behaviours                                             */
+/******************************************************************************************************/
+void Arduino_AlvikCarrier::beginBehaviours(){
+    prev_illuminator_state = illuminator_state;
+    behaviours = 0;
+    first_lift = true;
+}
+
+
+void Arduino_AlvikCarrier::updateBehaviours(){
+    if (behaviours|=1 == 1){
+        /*
+        if (isLifted()&&first_lift){
+            first_lift = false;
+            prev_illuminator_state = illuminator_state;
+            disableIlluminator();
+        }
+        if (isLifted()&&!first_lift) {
+            if (prev_illuminator_state!=0){
+                disableIlluminator();
+            }
+        }
+        if (!isLifted()&&!first_lift){
+            if (prev_illuminator_state!=0){
+                //first_lift = true;
+                enableIlluminator();
+            }
+        }
+        */
+       if (isLifted()&&first_lift){
+        //disableIlluminator();
+        setIlluminator(LOW);
+        first_lift=false;
+       }
+       else{
+        if (!isLifted()){
+            setIlluminator(prev_illuminator_state);
+        }
+        if (!isLifted()&&!first_lift){
+            first_lift = true;
+        }
+       }
+    }
+}
+
+void Arduino_AlvikCarrier::setBehaviour(const uint8_t behaviour, const bool enable){
+    if (enable){
+        behaviours |= behaviour;
+    }
+    else{
+        behaviours &= ~behaviour;
+    }
+}
+
+bool Arduino_AlvikCarrier::isLifted(){
+    if (getProximity()>LIFT_THRESHOLD){
+        return true;
+    }
+    else{
+        return false;
+    }
+}

src/Arduino_AlvikCarrier.h

@@ -37,6 +37,7 @@ class Arduino_AlvikCarrier{
 
         APDS9960 * apds9960;
         int bottom_red, bottom_green, bottom_blue, bottom_clear, bottom_proximity;
+        bool illuminator_state;
 
 
         Servo * servo_A;
@@ -79,6 +80,12 @@ class Arduino_AlvikCarrier{
         PidController * move_pid;
 
 
+        bool prev_illuminator_state;
+        uint8_t behaviours;
+        bool first_lift;
+
+
+
 
     public:        
         Kinematics * kinematics;
@@ -238,7 +245,10 @@ class Arduino_AlvikCarrier{
         bool isTargetReached();                                                         // get true if a movement is accomplished
         uint8_t getKinematicsMovement();                                                // get which kind of motion is running in kinematic control
 
-
+        void beginBehaviours();
+        void updateBehaviours();
+        void setBehaviour(const uint8_t behaviour, const bool enable);
+        bool isLifted();
 };
 
 #endif

src/definitions/pinout_definitions.h

@@ -95,7 +95,7 @@
 // Uart
 #define UART_TX PA9
 #define UART_RX PA10
-#define UART_BAUD 115200
+#define UART_BAUD 460800
 
 
 // Errors

src/definitions/robot_definitions.h

@@ -60,6 +60,14 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define MOVEMENT_MOVE 2
 
 
+#define LIFT_THRESHOLD 80
+
+#define ALL_BEHAVIOURS 255
+#define LIFT_ILLUMINATOR 1
+
+
+
+
 
 
 // Sensor fusioning parameters
@@ -80,7 +88,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 // Library version
 #define VERSION_BYTE_HIGH 0
 #define VERSION_BYTE_MID 2
-#define VERSION_BYTE_LOW 0
+#define VERSION_BYTE_LOW 1
 
 
 

src/motor_control/motor_control.cpp

@@ -42,6 +42,8 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
     angle = 0.0;
     reference = 0.0;
 
+    return_flag = false;
+
     trip = 0.0;
     iterations = 0.0;
     start_value = 0.0;
@@ -111,22 +113,28 @@ float MotorControl::getRPM(){
 
 
 bool MotorControl::setRPM(const float ref){
-    if ((ref<=MOTOR_LIMIT)&&(ref>=-MOTOR_LIMIT)){
-        reference = ref;
-        if (control_mode==CONTROL_MODE_LINEAR){
-            start_value=interpolation;
-            end_value=reference;
-            trip=0.0;
-            iterations=abs(end_value-start_value)/step_size;
-            step=1.0/iterations;
-            step_index=0;
-        }
-        else if(control_mode==CONTROL_MODE_NORMAL){
-            vel_pid->setReference(reference);
-        }
-        return true;
+    reference = ref;
+    return_flag = true;
+    if (ref>MOTOR_LIMIT){
+        reference=MOTOR_LIMIT;
+        return_flag = false;
+    }
+    if (ref<-MOTOR_LIMIT){
+        reference=-MOTOR_LIMIT;
+        return_flag = false;
+    }
+    if (control_mode==CONTROL_MODE_LINEAR){
+        start_value=interpolation;
+        end_value=reference;
+        trip=0.0;
+        iterations=abs(end_value-start_value)/step_size;
+        step=1.0/iterations;
+        step_index=0;
+    }
+    else if(control_mode==CONTROL_MODE_NORMAL){
+        vel_pid->setReference(reference);
     }
-    return false;
+    return return_flag;
 }
 
 void MotorControl::update(){

src/motor_control/motor_control.h

@@ -29,6 +29,8 @@ class MotorControl{
     private:
 
         uint8_t control_mode;
+        bool return_flag;
+
 
         float kp;
         float ki;