added experimental behaviour, illuminator&lift

Author: gbr1

examples/firmware_01/firmware_01.ino

@@ -31,12 +31,14 @@ uint8_t msg_size;
 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 tbehaviours=0;
 
 
 float left, right, value;
@@ -77,6 +79,7 @@ void setup(){
   tsensor=millis();
   timu=millis();
   tack=millis();
+  tbehaviours=millis();
 }
 
 void loop(){
@@ -181,6 +184,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;
     }
   }
 
@@ -256,6 +270,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
+version=0.2.1
 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,6 +173,7 @@ void Arduino_AlvikCarrier::updateAPDS(){
 }
 
 void Arduino_AlvikCarrier::setIlluminator(uint8_t value){
+    illuminator_state=value;
     digitalWrite(APDS_LED, value);
 }
 
@@ -811,3 +814,49 @@ 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){
+                first_lift = true;
+                enableIlluminator();
+            }
+        }
+    }
+}
+
+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/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