Merge pull request #6 from arduino-libraries/devel

0.1.2 - kinematics
arduino-libraries · Jan 31, 2024 · 65a837e · 65a837e
2 parents b2d1b4a + 109f41f
commit 65a837e
Show file tree

Hide file tree

Showing 7 changed files with 257 additions and 45 deletions.
diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino
@@ -9,6 +9,8 @@
     
 */
 
+// WIP -> preliminary firmware
+
 
 #include "Arduino_AlvikCarrier.h"
 #include "sensor_line.h"
@@ -35,20 +37,24 @@ unsigned long timu=0;
 
 
 float left, right, value;
+float linear, angular;
 uint8_t leds;
 
 uint8_t sensor_id = 0;
 
 
 uint8_t pid;
 float kp, ki, kd;
+float x, y, theta;
 
 uint8_t servo_A, servo_B;
 
 
 void setup(){
   Serial.begin(115200);
   alvik.begin();
+  alvik.disableIlluminator();
+  alvik.setLeds(COLOR_ORANGE);
   alvik.setLedBuiltin(HIGH);
   line.begin();
   tof.begin();
@@ -59,6 +65,7 @@ void setup(){
   alvik.serial->write(packeter.msg,msg_size);
 
   alvik.setLedBuiltin(LOW);
+  alvik.setLeds(COLOR_BLACK);
 
 
   code=0;
@@ -77,20 +84,51 @@ void loop(){
     switch (code){
       case 'J':
         packeter.unpacketC2F(code,left,right);
+        alvik.disableKinematicsMovement();
+        alvik.disablePositionControl();
         alvik.setRpm(left, right);
         break;
+
+      case 'V':
+        packeter.unpacketC2F(code,linear,angular);
+        alvik.disableKinematicsMovement();
+        alvik.disablePositionControl();
+        alvik.drive(linear,angular);
+        break;
+
       case 'W':
         packeter.unpacketC2B1F(code,label,control_type,value);
-        if ((label == 'L') && (control_type == 'V')) {
-          alvik.motor_control_left->setRPM(value);
+        alvik.disableKinematicsMovement();
+        if (label=='L'){
+          switch (control_type){
+            case 'V':
+              alvik.disablePositionControlLeft();
+              alvik.setRpmLeft(value);
+              break;
+            case 'P':
+              alvik.setPositionLeft(value);
+              break;
+            case 'Z':
+              alvik.resetPositionLeft(value);
+              break;
+          }
         }
-        else if ((label == 'R') && (control_type == 'V'))
-        {
-          alvik.motor_control_right->setRPM(value);
+        if (label=='R'){
+          switch (control_type){
+            case 'V':
+              alvik.disablePositionControlRight();
+              alvik.setRpmRight(value);
+              break;
+            case 'P':
+              alvik.setPositionRight(value);
+              break;
+            case 'Z':
+              alvik.resetPositionRight(value);
+              break;
+          }
         }
-
         break;
-
+      
       case 'S':
         packeter.unpacketC2B(code,servo_A,servo_B);
         alvik.setServoA(servo_A);
@@ -111,6 +149,23 @@ void loop(){
           alvik.setKPidRight(kp,ki,kd);
         }
         break;
+
+      case 'R':
+        packeter.unpacketC1F(code, value);
+        alvik.disablePositionControl();
+        alvik.rotate(value);
+        break;
+
+      case 'G':
+        packeter.unpacketC1F(code, value);
+        alvik.disablePositionControl();
+        alvik.move(value);
+        break;
+
+      case 'Z':
+        packeter.unpacketC3F(code, x, y, theta);
+        alvik.resetPose(x, y, theta);
+        break;
     }
   }
 
@@ -152,9 +207,33 @@ void loop(){
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
+    alvik.updateKinematics();
+    // joint speed
     msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
     alvik.serial->write(packeter.msg,msg_size);
-
+    // joint position
+    msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight());
+    alvik.serial->write(packeter.msg, msg_size);
+    // robot speed
+    msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
+    alvik.serial->write(packeter.msg, msg_size);
+    // pose
+    msg_size = packeter.packetC3F('s', alvik.getX(), alvik.getY(), alvik.getTheta());
+    alvik.serial->write(packeter.msg, msg_size);
+
+    if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){
+      if (alvik.isTargetReached()){
+        if (alvik.getKinematicsMovement()==MOVEMENT_ROTATE){
+          msg_size = packeter.packetC1B('x', 'R');
+        }
+        if (alvik.getKinematicsMovement()==MOVEMENT_MOVE){
+          msg_size = packeter.packetC1B('x', 'M');
+        }
+        alvik.serial->write(packeter.msg, msg_size);
+        //alvik.disableKinematicsMovement();
+      }
+
+    }
   }
 
   if (millis()-timu>10){
@@ -163,4 +242,4 @@ void loop(){
     msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ());
     alvik.serial->write(packeter.msg,msg_size);
   }
-}
+}
diff --git a/examples/kinematics/kinematics.ino b/examples/kinematics/kinematics.ino
@@ -9,63 +9,79 @@
     
 */
 
-// WIP
+// This example shows how to implement kinematics commands and retrieve odometry data from Arduino_AlvikCarrier class
 
 #include "Arduino_AlvikCarrier.h"
 
 Arduino_AlvikCarrier alvik;
 
 unsigned long tmotor=0;
 unsigned long ttask=0;
+unsigned long tled=0;
 uint8_t task=0;
+bool led_value = false;
 
 void setup() {
   Serial.begin(115200);
   alvik.begin();
   ttask=millis();
   tmotor=millis();
+  tled=millis();
   task=0;
 }
 
 void loop() {
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
-    alvik.kinematics->inverse(alvik.motor_control_left->getRPM(),alvik.motor_control_right->getRPM());
-    alvik.kinematics->updatePose();
+    alvik.updateKinematics();
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getLinearVelocity());
+    Serial.print(alvik.getLinearVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getAngularVelocity());
+    Serial.print(alvik.getAngularVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getX());
+    Serial.print(alvik.getX());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getY());
+    Serial.print(alvik.getY());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getTheta());
+    Serial.print(alvik.getTheta());
     Serial.print("\n");
   }
 
-  if (millis()-ttask>2000){
+  if ((millis()-ttask>5000)||alvik.isTargetReached()){
     ttask=millis();
     switch (task){
       case 0:
         alvik.rotate(90);
         break;
       case 1:
-        alvik.drive(40,0);
+        alvik.disableKinematicsMovement();
+        alvik.drive(0,0);
         break;
       case 2:
-        alvik.rotate(-90);
+        alvik.move(100);
         break;
       case 3:
-        alvik.drive(-40,0);
+        alvik.disableKinematicsMovement();
+        alvik.drive(0,0);
         break;
     }
     task++;
     if (task>3){
       task=0;
     }
   }
-
+
+  if (millis()-tled>200){
+    tled=millis();
+    led_value=!led_value;
+    if (task==1){
+      alvik.setLedLeftGreen(false);
+      alvik.setLedLeftRed(led_value);
+    }
+    if (task==3){
+      alvik.setLedLeftRed(false);
+      alvik.setLedLeftGreen(led_value);
+    }
+  }
 }
diff --git a/library.properties b/library.properties
@@ -1,5 +1,5 @@
 name=Arduino Alvik Carrier
-version=0.1.1
+version=0.1.2
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <[email protected]>
 sentence=Library and firmware for Arduino Alvik Carrier board