Merge remote-tracking branch 'refs/remotes/origin/master'

emb/command.h

@@ -3,13 +3,15 @@
 
 #include <stdint.h>
 
+#define NUM_ARGS 3
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
      struct command {
 	  char comm_ch; /* Character of command (ie 's', 'v', 'd') */
-	  int16_t arg[2];
+	  int16_t arg[NUM_ARGS];
      };
      
      uint8_t uart_add_ch(char c);

emb/servos.cpp

@@ -4,7 +4,6 @@
 #include <avr/interrupt.h>
 #include <stdint.h>
 
-#define NUM_SERVOS 3
 #define MIN_PULSE 999
 #define MAX_PULSE 4999
 
@@ -18,6 +17,7 @@ struct Servo
     const uint8_t pinMask;          // the pin mask for the servo
     uint16_t tempValue;             // stores a temporary value before it's written to the servo
     uint16_t value;                 // current pulse width value
+    int16_t angle;                  // the angle in arc-minutes
 };
 
 static Servo servos[NUM_SERVOS] = {
@@ -43,7 +43,23 @@ void initServos()
 void setServoAngle(uint8_t servo, int16_t arcMin)
 {
     if (0 <= servo && servo < NUM_SERVOS)
+    {
+        if (arcMin < -180 * 60)
+            arcMin = -180 * 60;
+        else if (arcMin > 180 * 60)
+            arcMin = 180 * 60;
+        
         servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
+        servos[servo].angle = arcMin;
+    }
+}
+
+int16_t getServoAngle(uint8_t servo)
+{
+    if (0 <= servo && servo < NUM_SERVOS)
+        return servos[servo].angle;
+    else
+        return 0;
 }
 
 void updateServos()

emb/servos.hpp

@@ -3,6 +3,8 @@
 
 #include <stdint.h>
 
+#define NUM_SERVOS 3
+
 /**
  * Initialises the servos.
  */
@@ -16,6 +18,11 @@ void initServos();
  */
 void setServoAngle(uint8_t servo, int16_t arcMin);
 
+/**
+ * Gets the angle of a servo.
+ */
+int16_t getServoAngle(uint8_t servo);
+
 /**
  * Writes the servo values out to the servos.
  */

emb/state-controller.cpp

@@ -6,19 +6,39 @@
 
 enum State
 {
-    STATE_DELAY, STATE_READY
+     STATE_READY = 0, STATE_DELAY
 };
 
 static State state = STATE_READY;
 static uint32_t delayEnd = 0;
 
+static int16_t servoVelocities[NUM_SERVOS];
+static uint32_t velocitiesEndMillis[NUM_SERVOS];
+static int16_t startAngles[NUM_SERVOS];
+static uint32_t startMillis[NUM_SERVOS];
+
 void parseCommand(command *comm)
 {
+    uint32_t currentMillis = getCurrentMillis();
+    
     switch (comm->comm_ch)
     {
         case 's':
-            setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
-            updateServos();
+            if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
+            { 
+                setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
+                updateServos();
+                servoVelocities[comm->arg[0] - 1] = 0;
+            }
+            break;
+        case 'v':
+            if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
+            {
+                servoVelocities[comm->arg[0] - 1] = comm->arg[1];
+                velocitiesEndMillis[comm->arg[0] - 1] = currentMillis + comm->arg[2];
+                startAngles[comm->arg[0] - 1] = getServoAngle(comm->arg[0] - 1);
+                startMillis[comm->arg[0] - 1] = currentMillis;
+            }
             break;
         case 'd':
             delayEnd = getCurrentMillis() + comm->arg[0];
@@ -29,6 +49,8 @@ void parseCommand(command *comm)
 
 void updateState()
 {
+    uint32_t currentMillis = getCurrentMillis();
+    
     switch (state)
     {
         case STATE_READY:
@@ -44,11 +66,29 @@ void updateState()
             break;
         }
         case STATE_DELAY:
-            if (getCurrentMillis() > delayEnd)
+            if (currentMillis > delayEnd)
                 state = STATE_READY;
             
             break;
     }
+    
+    for (uint8_t i = 0; i < NUM_SERVOS; i++)
+    {
+        if (servoVelocities[i] != 0)
+        {
+            if (currentMillis < velocitiesEndMillis[i])
+            {
+                int16_t newAngle = startAngles[i] + (int32_t) servoVelocities[i] * 60 * (int32_t) (currentMillis - startMillis[i]) / 1000;
+                setServoAngle(i, newAngle);
+            }
+            else
+            {
+                setServoAngle(i, startAngles[i] + (int32_t) servoVelocities[i] * 60 * (int32_t) (velocitiesEndMillis[i] - startMillis[i]) / 1000);
+                servoVelocities[i] = 0;
+            }
+        }
+    }
+    updateServos();
 }