Ik edits

arm_main/include/ArmJoint.h

@@ -9,6 +9,12 @@
 #include <Arduino.h>
 #include <AS5047P.h>
 
+enum SetpointType {
+    SETPOINT_ANGLE,
+    SETPOINT_VELOCITY
+};
+
+
 const float PRECISION = 1;
 
 const float MAX_SPEED = 500;
@@ -27,43 +33,65 @@ const float kD = 0.0;
  */
 float clamp_angle(float angle);
 
+/**
+ * @brief enforces MAX_SPEED and MIN_SPEED motor RPM limits
+ * 
+ * @param velocity RPM
+ * @return float clamped RPM
+ */
 float clamp_velocity(float velocity);
 
 
 class ArmJoint {
 //    private:
    public:
-    float zeroAngle;
-    float targetAngle;
-    float lastEffectiveAngle;
-    float lastEncoderAngle;
-    long lastEncoderReadTime;
-    float integral;
-    float prevError;
+    float zeroAngle;            // Raw encoder angle reading that corresponds to 0 degrees
     float minAngle;
     float maxAngle;
+
+    SetpointType setpointType;  // Angle (deg) or angular velocity (deg/s)
+    float targetAngle;          // Degrees from zeroAngle
+    float lastEffectiveAngle;   // Last encoder angle reading, adjusted with zeroAngle
+    float lastEncoderAngle;     // Last raw encoder angle reading
+    long lastEncoderReadTime;   // millis() value of last encoder read
+    float targetVelocity;       // Degrees per second
+    float lastREVVelocity;      // Last motor RPM reported directly by the Sparkmax
+    float lastDegSVelocity;     // Last joint velocity in deg/s (converted from lastREVVelocity)
+    long lastREVReadTime;       // millis() value of last Sparkmax velocity read
+
+    float integral;
+    float prevError;
+
     long timeToGoal;
     long goalTime;  // millis() value when the arm should be at its goal
+
     int gearRatio;
     bool inverted;
     AS5047P* encoder;
 
     double pid(double pTargetAngle);
 
-    public:
+   public:
     ArmJoint(AS5047P* setEncoder, float setZeroAngle = 0, float setMinAngle = -115, float setMaxAngle = 115, int setGearRatio = 1, bool setInverted = false);
     float readAngle();
+    void readREVVelocity(int rpm);
     float updateIKMotion();
 
     inline void setTargetAngle(float angle) {
         targetAngle = angle;
+        setpointType = SETPOINT_ANGLE;
         if (targetAngle < minAngle) {
             targetAngle = minAngle;
         } else if (targetAngle > maxAngle) {
             targetAngle = maxAngle;
         }
     }
 
+    inline void setTargetVelocity(float velocity) {
+        targetVelocity = velocity;
+        setpointType = SETPOINT_VELOCITY;
+    }
+
     inline bool checkDuty(float duty) {
         if (inverted)
             duty = -duty;

arm_main/include/AstraArm.h

@@ -43,6 +43,7 @@ class AstraArm {
    public:
     AstraArm(ArmJoint* setJoints[]);
     void setTargetAngles(float angle0, float angle1, float angle2, float angle3);
+    void setTargetVelocities(float velocities[4]);
     void updateIKMotion();  // Functions same as updateForAcceleration()
     void runDuty(float dutyCycles[4]);
 

arm_main/platformio.ini

@@ -11,18 +11,35 @@
 ; [env:adafruit_feather_esp32_v2]
 ; platform = espressif32
 ; board = adafruit_feather_esp32_v2
-[env:esp32doit-devkit-v1]
+[env:arm_main_prod]
 platform = espressif32
 board = esp32doit-devkit-v1
 framework = arduino
 monitor_speed = 115200
 monitor_filters = send_on_enter
 monitor_echo = true
 build_flags = 
-	-D ARM
-	; -D FEEDBACK_PRECISION=2
-	; -D DEBUG
+    -D ARM
+    ; -D FEEDBACK_PRECISION=2
+    ; -D DEBUG
 lib_deps = 
-	https://github.com/SHC-ASTRA/rover-Embedded-Lib#main
-	handmade0octopus/ESP32-TWAI-CAN@^1.0.1
-	jonas-merkle/AS5047P@^2.2.2
+    https://github.com/SHC-ASTRA/astra-embedded-lib#v1.0.2
+    handmade0octopus/ESP32-TWAI-CAN@^1.0.1
+    jonas-merkle/AS5047P@^2.2.2
+
+[env:arm_main_dev]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+monitor_speed = 115200
+monitor_filters = send_on_enter
+monitor_echo = true
+build_flags = 
+    -D ARM
+    ; -D FEEDBACK_PRECISION=2
+    ; -D DEBUG
+lib_deps = 
+    symlink://../../astra-embedded-lib
+    symlink://../../unilib
+    handmade0octopus/ESP32-TWAI-CAN@^1.0.1
+    jonas-merkle/AS5047P@^2.2.2

arm_main/src/ArmJoint.cpp

@@ -15,6 +15,8 @@ ArmJoint::ArmJoint(AS5047P* setEncoder, float setZeroAngle, float setMinAngle, f
     inverted = setInverted;
 
     targetAngle = 0;
+    targetVelocity = 0;
+    setpointType = SETPOINT_ANGLE;
     lastEncoderAngle = 0;
     lastEncoderReadTime = 0;
     minAngle = setMinAngle;
@@ -33,6 +35,17 @@ float ArmJoint::readAngle() {
     return lastEffectiveAngle;
 }
 
+void ArmJoint::readREVVelocity(int rpm) {
+    rpm *= -1;
+    lastREVVelocity = rpm;
+    lastREVReadTime = millis();
+
+    // Convert motor RPM to joint deg/s
+    // Lowest gear ratio is 468, largest is 5000
+    // Meaning, in practice, values range from 6.0 to 128.2
+    lastDegSVelocity = (float(rpm) * 6.0) / float(gearRatio);
+}
+
 double ArmJoint::pid(double pTargetAngle) {
     double error = clamp_angle(pTargetAngle - lastEffectiveAngle);
     if (abs(error) < PRECISION)
@@ -54,19 +67,38 @@ float ArmJoint::updateIKMotion() {
     if (static_cast<long>(millis()) - lastEncoderReadTime > dt)
         return 0;  // Don't move if encoder data is too old
 
-    double pidTargetAngle = targetAngle;  // Will come from S-curve
-
-    float motorRPM = pid(targetAngle);
-    if (motorRPM == 0) {
-        return 0;
+    // Calculate motor RPM required to achieve either absolute angle setpoint or velocity setpoint
+    float motorRPM = 0;
+    if (setpointType == SETPOINT_ANGLE) {
+        double pidTargetAngle = targetAngle;  // Will come from S-curve
+
+        motorRPM = pid(targetAngle);
+    } else {
+        if (targetVelocity == 0)
+            return 0;
+
+        // Perform bounds checking -- joint shall not exceed min/max angles within one second
+        float projectedAngle = lastEffectiveAngle + targetVelocity;
+        if (lastEffectiveAngle > maxAngle || lastEffectiveAngle < minAngle) {
+            targetVelocity = 0;
+        } else if (projectedAngle < minAngle) {
+            targetVelocity = minAngle - lastEffectiveAngle;
+        } else if (projectedAngle > maxAngle) {
+            targetVelocity = maxAngle - lastEffectiveAngle;
+        }
+
+        // Convert deg/sec to motor RPM
+        motorRPM = (targetVelocity * 60.0 / 360.0) * float(gearRatio);
     }
+    if (motorRPM == 0)
+        return 0;
 
-    motorRPM = clamp_velocity(motorRPM);  // Keep non-zero velocity within MIN_SPEED and MAX_SPEED
+    motorRPM = clamp_velocity(motorRPM);  // Bound motor RPM within MIN_SPEED and MAX_SPEED
 
-    if (inverted) {
-        motorRPM = -motorRPM;
-    }
-    return -1 * motorRPM;
+    if (inverted)
+        motorRPM *= -1;
+
+    return -1 * motorRPM;  // Motors turn in the opposite direction of our joint angle convention
 }
 
 

arm_main/src/AstraArm.cpp

@@ -24,13 +24,24 @@ void AstraArm::setTargetAngles(float angle0, float angle1, float angle2, float a
     for (int i = 0; i < 4; i++) {
         joints[i]->timeToGoal = timeToGoal;
         joints[i]->goalTime = long(millis()) + timeToGoal;
+        joints[i]->setpointType = SETPOINT_ANGLE;
     }
     joints[0]->setTargetAngle(angle0);
     joints[1]->setTargetAngle(angle1);
     joints[2]->setTargetAngle(angle2);
     joints[3]->setTargetAngle(angle3);
 }
 
+void AstraArm::setTargetVelocities(float velocities[4]) {
+    isIKMode = true;
+    for (int i = 0; i < 4; i++) {
+        joints[i]->timeToGoal = timeToGoal;
+        joints[i]->goalTime = long(millis()) + timeToGoal;
+        joints[i]->setpointType = SETPOINT_VELOCITY;
+        joints[i]->setTargetVelocity(velocities[i]);
+    }
+}
+
 void AstraArm::updateIKMotion() {
     for (int i = 0; i < 4; i++) {
         joints[i]->readAngle();
@@ -49,15 +60,13 @@ void AstraArm::updateIKMotion() {
         }
         if (wasLimitViolation)
             sendDuty(newDutyCycles[0], newDutyCycles[1], newDutyCycles[2], newDutyCycles[3]);
-
-        return;
-    }
-
-    float velocities[4] = {0};
-    for (int i = 0; i < 4; i++) {
-        velocities[i] = joints[i]->updateIKMotion();
+    } else {
+        float velocities[4] = {0};
+        for (int i = 0; i < 4; i++) {
+            velocities[i] = joints[i]->updateIKMotion();
+        }
+        sendVelocity(velocities[0], velocities[1], velocities[2], velocities[3]);
     }
-    sendVelocity(velocities[0], velocities[1], velocities[2], velocities[3]);
 }
 
 void AstraArm::runDuty(float dutyCycles[4]) {

arm_main/src/main.cpp

@@ -49,8 +49,8 @@ AS5047P ax3_encoder(ENCODER_AXIS3_PIN, SPI_BUS_SPEED);
 // ArmJoint(AS5047P* setEncoder, float setZeroAngle, float setMinAngle, float setMaxAngle, int setGearRatio, bool setInverted);
 ArmJoint axis0(&ax0_encoder, 179, -179, 135, 468);  // 64:1 gearbox, 16:117 small and big gears
 ArmJoint axis1(&ax1_encoder, 55, -60, 90, 5000);
-ArmJoint axis2(&ax2_encoder, 352, -115, 115, 3750);
-ArmJoint axis3(&ax3_encoder, 7.5, -90, 110, 2500);
+ArmJoint axis2(&ax2_encoder, 245.3, -115, 115, 3750);
+ArmJoint axis3(&ax3_encoder, 322.7, -90, 110, 2500);
 ArmJoint* joints[] = {&axis0, &axis1, &axis2, &axis3};
 
 AstraArm arm(joints);
@@ -194,17 +194,18 @@ void loop() {
         vicCAN.send(CMD_POWER_VOLTAGE, vBatt * 100, v12 * 100, v5 * 100, v33 * 100);
     }
 
-    if (millis() - lastFeedback >= 100)
+    if (millis() - lastFeedback >= 250)
     {
         lastFeedback = millis();
         vicCAN.send(CMD_ARM_ENCODER_ANGLES, axis0.lastEffectiveAngle * 10, axis1.lastEffectiveAngle * 10, axis2.lastEffectiveAngle * 10, axis3.lastEffectiveAngle * 10);
+        vicCAN.send(59, axis0.lastDegSVelocity * 100, axis1.lastDegSVelocity * 100, axis2.lastDegSVelocity * 100, axis3.lastDegSVelocity * 100);
 #ifdef DEBUG
         Serial.printf("Axis0: %f\tAxis1: %f\tAxis2: %f\tAxis3: %f\n", axis0.lastEffectiveAngle, axis1.lastEffectiveAngle, axis2.lastEffectiveAngle, axis3.lastEffectiveAngle);
 #endif
     }
 
     // Safety timeout if no ctrl command for 2 seconds
-    if (millis() - lastCtrlCmd > 10000)
+    if (millis() - lastCtrlCmd > 2000)
     {
         lastCtrlCmd = millis();
         arm.stop();
@@ -292,6 +293,9 @@ void loop() {
                     COMMS_UART.println("brake,on");
             }
         }
+
+        // Submodule-specific
+
         else if (commandID == CMD_ARM_IK_CTRL) {
             if (canData.size() == 4) {
 #ifdef ARM_DEBUG
@@ -330,6 +334,16 @@ void loop() {
                 arm.runDuty(speeds);
             }
         }
+        else if (commandID == 43) {  // IK Velocity setpoint
+            if (canData.size() == 4) {
+                lastCtrlCmd = millis();
+                float velocities[4] = {0};
+                for (int i = 0; i < 4; i++) {
+                    velocities[i] = canData[i] == 0 ? 0 : canData[i] / 10.0;
+                }
+                arm.setTargetVelocities(velocities);
+            }
+        }
     }
 
 
@@ -472,16 +486,23 @@ void loop() {
         std::vector<String> args = {};  // Initialize empty vector to hold separated arguments
         parseInput(input, args);   // Separate `input` by commas and place into args vector
 
-#ifdef ARM_DEBUG
-        Serial.println("|------------------------------------------------------|");
-        Serial.print("| From Motor MCU Recieved: ");
-#endif
-        Serial.print("Motor MCU:\t");
+        Serial.print("Motor MCU: ");
         Serial.println(input);
 
         if (checkArgs(args, 4) && args[0] == "motorstatus") {
             vicCAN.send(CMD_REVMOTOR_FEEDBACK, args[1].toInt(), args[2].toInt(), args[3].toInt(), args[4].toInt());
         }
+
+        else if (checkArgs(args, 3) && args[0] == "motormotion") {
+            int motorId = args[1].toInt();
+            int motorPos = args[2].toInt();
+            int motorRPM = args[3].toInt();
+            vicCAN.send(58, motorId, motorPos, motorRPM);
+            if (motorId >= 1 && motorId <= 3)
+                joints[motorId]->readREVVelocity(motorRPM);
+            else if (motorId == 4)  // Axis Zero's ID is 4, not 0
+                joints[0]->readREVVelocity(motorRPM);
+        }
     }
 }
 

arm_motors/platformio.ini

@@ -11,7 +11,7 @@
 ; [env:adafruit_feather_esp32_v2]
 ; platform = espressif32
 ; board = adafruit_feather_esp32_v2
-[env:esp32doit-devkit-v1]
+[env:arm_motor_prod]
 platform = espressif32
 board = esp32doit-devkit-v1
 framework = arduino
@@ -22,5 +22,21 @@ build_flags =
     -D MOTORMCU
     ; -D DEBUG
 lib_deps = 
-    https://github.com/SHC-ASTRA/rover-Embedded-Lib#main
+    https://github.com/SHC-ASTRA/astra-embedded-lib#v1.0.2
+    handmade0octopus/ESP32-TWAI-CAN@^1.0.1
+
+
+[env:arm_motor_dev]
+platform = espressif32
+board = esp32doit-devkit-v1
+framework = arduino
+monitor_speed = 115200
+monitor_filters = send_on_enter
+monitor_echo = true
+build_flags =
+    -D MOTORMCU
+    ; -D DEBUG
+lib_deps = 
+    symlink://../../astra-embedded-lib
+    symlink://../../unilib
     handmade0octopus/ESP32-TWAI-CAN@^1.0.1