Merge pull request #43 from Armandpl/41-improve-firmware-protocol

41 improve firmware protocol

.github/workflows/lint.yml

@@ -18,6 +18,9 @@ jobs:
       - name: Set up Python environment
         uses: actions/setup-python@v1
         with:
-          python-version: "3.7"
+          python-version: "3.8"
       - name: flake8 Lint
         uses: py-actions/flake8@v1
+        with:
+          ignore: "E203,E402,E501,F401,F841"
+          exclude: "logs/*,data/*,furuta_gym/logging/protobuf/*"

.pre-commit-config.yaml

@@ -55,7 +55,7 @@ repos:
             "--extend-ignore",
             "E203,E402,E501,F401,F841",
             "--exclude",
-            "logs/*,data/*",
+            "logs/*,data/*,furuta_gym/logging/protobuf/*",
           ]
 
   # yaml formatting
@@ -64,7 +64,6 @@ repos:
     hooks:
       - id: prettier
         types: [yaml]
-
 #   # jupyter notebook cell output clearing
 #   - repo: https://github.com/kynan/nbstripout
 #     rev: 0.5.0
@@ -84,4 +83,4 @@ repos:
 #           [
 #             "--extend-ignore=E203,E402,E501,F401,F841",
 #             "--exclude=logs/*,data/*",
-#           ]
+#           ]

furuta_gym/robot.py

@@ -0,0 +1,34 @@
+import struct
+
+import matplotlib.pyplot as plt
+import numpy as np
+import serial
+import simple_pid
+
+
+class Robot:
+    def __init__(self, device="dev/ttyACM0", baudrate=921600):
+        self.ser = serial.Serial(device, baudrate)
+
+    def step(self, motor_command: float):
+        """motor command is a float between -1 and 1."""
+        direction = motor_command < 0
+        # convert motor command to 16 bit unsigned int
+        int_motor_command = int(np.abs(motor_command) * (2**16 - 1))
+        tx = b"\x10\x02"  # start sequence
+        tx += b"\x01"  # command type = STEP = 0x01
+        tx += struct.pack("<?H", direction, int_motor_command)
+        self.ser.write(tx)
+        resp = self.ser.read(8)
+        motor_angle, pendulum_angle = struct.unpack("<ff", resp)
+        return motor_angle, pendulum_angle
+
+    def reset_encoders(self):
+        tx = b"\x10\x02"  # start sequence
+        tx += b"\x00"  # command type = RESET = 0x00
+        tx += b"\x00\x00\x00"  # three empty bytes to have fixed len packets
+        self.ser.write(tx)
+
+    def close(self):
+        self.step(0)
+        self.ser.close()

pyproject.toml

@@ -16,6 +16,9 @@ matplotlib = "3.7.3"
 pre-commit = "^3.5.0"
 
 
+[tool.poetry.group.dev.dependencies]
+pytest = "^7.4.4"
+
 [build-system]
 requires = ["poetry-core"]
 build-backend = "poetry.core.masonry.api"

robot/firmware/firmware.ino

@@ -1,6 +1,11 @@
 #include <stdint.h>
 #include <Encoder.h>
 
+// protocol def
+const uint8_t PACKET_SIZE = 6;
+const uint8_t RESET = 0;
+const uint8_t STEP = 1;
+
 // Pin definitions
 const uint8_t MOTOR_DRIVER = 0; // 0 = MC33926
 const uint8_t MOTOR_IN1 = 1; // which way to go
@@ -23,41 +28,30 @@ const float PENDULUM_ENCODER_CPR = 8192;
 Encoder motorEncoder(MOTOR_ENC_A, MOTOR_ENC_B);
 Encoder pendulumEncoder(PENDULUM_ENC_A, PENDULUM_ENC_B);
 
+volatile unsigned long lastCommandReceived = 0;
+const unsigned long COMMAND_TIMEOUT = 500; // ms
 
-void processMotorCommand(int16_t motor_command) {
-  // clamp motor command between -255 and 255
-  if (motor_command > 255) {
-    motor_command = 255;
-  }
-  else if (motor_command < -255) {
-    motor_command = -255;
-  }
-
-  // TODO deadzone
-  uint8_t scale_motor_command = 0;
-  if (motor_command != 0){
-    scale_motor_command = int(0.3 * 255 + 0.7 * abs(motor_command));
-  }
 
+void processMotorCommand(uint16_t motor_command, bool direction) {
   if (MOTOR_DRIVER == 0) { // MC33926
-    if (motor_command > 0) {
+    if (direction) {
       digitalWrite(MOTOR_IN1, LOW);
       digitalWrite(MOTOR_IN2, HIGH);
     }
     else {
       digitalWrite(MOTOR_IN1, HIGH);
       digitalWrite(MOTOR_IN2, LOW);
     }
-    analogWrite(MOTOR_D2, scale_motor_command);
+    analogWrite(MOTOR_D2, motor_command);
   }
   else if (MOTOR_DRIVER == 1) { // TB9051-FTG
-    if (motor_command > 0) {
-      analogWrite(PWM1, scale_motor_command);
+    if (direction) {
+      analogWrite(PWM1, motor_command);
       analogWrite(PWM2, 0);
     }
     else {
       analogWrite(PWM1, 0);
-      analogWrite(PWM2, scale_motor_command);
+      analogWrite(PWM2, motor_command);
     }
   }
 }
@@ -74,40 +68,65 @@ void setup() {
     pinMode(PWM1, OUTPUT);
     pinMode(PWM2, OUTPUT);
   }
-  analogWriteResolution(8);
+  analogWriteResolution(16);
 
   // setup serial
-  Serial.begin(57600);
+  Serial.begin(921600);
 }
 
 
 void loop() {
-  // if at least two bytes (== one motor command)
-  // in the serial buffer
-  if (Serial.available() >= 2){
-    // read the two bytes
-    uint8_t low_byte = Serial.read();
-    uint8_t high_byte = Serial.read();
-    // combine the two bytes to one int16_t
-    int16_t motor_command = (high_byte << 8) | low_byte;
-    // process the motor command
-    processMotorCommand(motor_command);
-
-    // read the motor and pendulum encoder values
-    // convert to angles in radians
-    // and send them back as two floats
-
-    // read the motor and pendulum encoder values
-    long motorEncoderValue = motorEncoder.read();
-    long pendulumEncoderValue = pendulumEncoder.read();
-
-    // convert to angles in radians
-    float motorAngle = (2 * PI * motorEncoderValue) / MOTOR_ENCODER_CPR;
-    float pendulumAngle = (2 * PI * pendulumEncoderValue) / PENDULUM_ENCODER_CPR;
-
-    // send them back as two floats
-    Serial.write((uint8_t*)&motorAngle, sizeof(motorAngle));
-    Serial.write((uint8_t*)&pendulumAngle, sizeof(pendulumAngle));
+  if (millis() - lastCommandReceived > COMMAND_TIMEOUT) {
+    processMotorCommand(0, true); // kill motor
   }
 
+  if (Serial.available() >= PACKET_SIZE) {
+    // check for start sequence
+    if(Serial.read() != 0x10){
+      return;
+    }
+    if(Serial.read() != 0x02){
+      return;
+    }
+
+    lastCommandReceived = millis();
+
+    // valid packet, read the command type
+    uint8_t command = Serial.read();
+    if (command == RESET) {
+      // reset encoders
+      motorEncoder.write(0);
+      pendulumEncoder.write(0);
+      processMotorCommand(0, true); // kill motor
+      // Discard unnecessary bytes from the serial buffer
+      for (int i = 0; i < (PACKET_SIZE - 3); i++) {
+        Serial.read();
+      }
+    }
+    else if (command == STEP) {
+      // read the two bytes
+      bool direction = Serial.read();
+      uint16_t motor_command;
+      Serial.readBytes((char*)&motor_command, sizeof(motor_command));
+
+      // process the motor command
+      processMotorCommand(motor_command, direction);
+
+      // read the motor and pendulum encoder values
+      // convert to angles in radians
+      // and send them back as two floats
+
+      // read the motor and pendulum encoder values
+      long motorEncoderValue = motorEncoder.read();
+      long pendulumEncoderValue = pendulumEncoder.read();
+
+      // convert to angles in radians
+      float motorAngle = (2 * PI * motorEncoderValue) / MOTOR_ENCODER_CPR;
+      float pendulumAngle = (2 * PI * pendulumEncoderValue) / PENDULUM_ENCODER_CPR;
+
+      // send them back as two floats
+      Serial.write((uint8_t*)&motorAngle, sizeof(motorAngle));
+      Serial.write((uint8_t*)&pendulumAngle, sizeof(pendulumAngle));
+    }
+  }
 }