Merge pull request #56 from Evanyl/gking/27/isolation_integration

Gking/27/isolation integration

fw/src/app/arm.cpp

@@ -15,7 +15,7 @@
 #define ARM_TOPDOWN_ANGLE 0.0
 #define ARM_RAMP_ANGLE 5.0
 
-#define ARM_NAV_RATE 750
+#define ARM_NAV_RATE 1500
 #define ARM_HOMING_RATE 100
 #define ARM_STARTING_RATE 250
 

fw/src/app/belts.cpp

@@ -10,8 +10,8 @@
 *                               C O N S T A N T S                              *
 *******************************************************************************/ 
 
-#define BELTS_NAV_RATE 750
-#define BELTS_STARTING_RATE 100
+#define BELTS_NAV_RATE 1500
+#define BELTS_STARTING_RATE 200
 
 /*******************************************************************************
 *                      D A T A    D E C L A R A T I O N S                      *
@@ -23,7 +23,9 @@ typedef struct
     belts_state_E state;
     belts_state_E des_state;
     uint16_t top_belt_steps;
+    uint8_t top_belt_dir;
     uint16_t bottom_belt_steps;
+    uint8_t bottom_belt_dir;
 } belts_data_S;
 
 /*******************************************************************************
@@ -41,7 +43,9 @@ static belts_data_S belts_data =
     .state = BELTS_STATE_IDLE,
     .des_state = BELTS_STATE_IDLE,
     .top_belt_steps = 0,
+    .top_belt_dir = 1,
     .bottom_belt_steps = 0,
+    .bottom_belt_dir = 1,
 
 };
 
@@ -89,13 +93,13 @@ static belts_state_E belts_update_state(belts_state_E curr_state)
         case BELTS_STATE_ACTIVE:
             top = stepper_command(STEPPER_BELT_TOP,  
                                 belts_data.top_belt_steps, 
-                                1, 
+                                belts_data.top_belt_dir, 
                                 750, 
                                 belts_data.top_belt_steps*0.1, 
                                 100);
             bottom = stepper_command(STEPPER_BELT_BOTTOM,  
                                 belts_data.bottom_belt_steps, 
-                                1, 
+                                belts_data.bottom_belt_dir, 
                                 750, 
                                 belts_data.bottom_belt_steps*0.1, 
                                 100);
@@ -173,6 +177,16 @@ void belts_core_comms_setDesState(uint8_t argNumber, char* args[])
 
 void belts_core_comms_setSteps(uint8_t argNumber, char* args[])
 {
-    belts_data.top_belt_steps = atoi(args[0]);
-    belts_data.bottom_belt_steps = atoi(args[1]);
+    belts_data.top_belt_steps = abs(atoi(args[0]));
+    belts_data.bottom_belt_steps = abs(atoi(args[1]));
+    belts_data.top_belt_dir = 1;
+    belts_data.bottom_belt_dir = 1;
+    if (atoi(args[0]) < 0)
+    {
+        belts_data.top_belt_dir = 0;
+    }
+    if (atoi(args[1]) < 0)
+    {
+        belts_data.bottom_belt_dir = 0;   
+    }
 }

fw/src/app/depositor.cpp

@@ -12,25 +12,31 @@
 *                               C O N S T A N T S                              *
 *******************************************************************************/ 
 
-#define DEPOSITOR_OPEN_STEPS   255
-#define DEPOSITOR_OPEN_RATE    25
-#define DEPOSITOR_OPEN_ANGLE   0.0 //-90.0 achieves full open
-
 #define DEPOSITOR_CLOSE_STEPS  255
 #define DEPOSITOR_CLOSE_RATE   255
 #define DEPOSITOR_CLOSE_ANGLE  62.0 // Fully closed
 
+#define DEPOSITOR_OPEN_STEPS   200
+#define DEPOSITOR_OPEN_RATE    25
+#define DEPOSITOR_OPEN_ANGLE   0.0 //-90.0 achieves full open
+
+#define DEPOSITOR_SLIT_ANGLE   DEPOSITOR_CLOSE_ANGLE - 12.0
+#define DEPOSITOR_SLIT_RATE    255
+#define DEPOSITOR_SLIT_STEPS   5
+
 #define DEPOSITOR_ARM_HOME_ANGLE 0.0
 #define DEPOSITOR_ARM_CENTER_ANGLE 75.0
 #define DEPOSITOR_ARM_SWEEP_ANGLE  110.0
 #define DEPOSITOR_ARM_RAMP_ANGLE 5.0 // Degrees
 
-#define DEPOSITOR_ARM_NAV_RATE 750
+#define DEPOSITOR_ARM_NAV_RATE 1500
 #define DEPOSITOR_ARM_HOME_RATE 150
 #define DEPOSITOR_ARM_STARTING_RATE 250 // Steps per second
 
 #define DEPOSITOR_ARM_CW 1
 
+#define DEPOSITOR_JOSTLE_MOVEMENTS 20
+
 /*******************************************************************************
 *                      D A T A    D E C L A R A T I O N S                      *
 *******************************************************************************/ 
@@ -39,6 +45,8 @@ typedef struct
 {
     struct pt  thread;
     depositor_state_E state;
+    int8_t jostle_counter;
+    float jostle_routine[DEPOSITOR_JOSTLE_MOVEMENTS];
 } depositor_data_S;
 
 /*******************************************************************************
@@ -56,6 +64,8 @@ static depositor_state_E depositor_update_state(depositor_state_E curr_state);
 static depositor_data_S depositor_data = 
 {
     .state = DEPOSITOR_STATE_HOMING,
+    .jostle_counter = 0,
+    .jostle_routine = {0,1,-1,0,0,0,-1,1,0,0,0,1,-1,0,0,0,-1,1,0,0},
 };
 
 /*******************************************************************************
@@ -121,11 +131,11 @@ static depositor_state_E depositor_update_state(depositor_state_E curr_state)
             }
             else
             {
-                next_state = DEPOSITOR_STATE_DROPPING;
+                next_state = DEPOSITOR_STATE_CENTERING;
             }
             break;
 
-        case DEPOSITOR_STATE_DROPPING:
+        case DEPOSITOR_STATE_CENTERING:
             if (stepper_commandAngle(STEPPER_DEPOSITOR, 
                                      DEPOSITOR_ARM_CENTER_ANGLE, 
                                      DEPOSITOR_ARM_RAMP_ANGLE, 
@@ -135,16 +145,53 @@ static depositor_state_E depositor_update_state(depositor_state_E curr_state)
                 // do nothing, navigating to the center
             }
             else
+            {
+                if (servo_command(SERVO_DEPOSITOR,
+                                  DEPOSITOR_SLIT_ANGLE,
+                                  DEPOSITOR_SLIT_STEPS,
+                                  DEPOSITOR_SLIT_RATE) == false)
+                {
+                    // do nothing
+                }
+                else
+                {
+                    next_state = DEPOSITOR_STATE_DROPPING;
+                }
+            }
+            break;
+
+        case DEPOSITOR_STATE_DROPPING:
+            if (depositor_data.jostle_counter < DEPOSITOR_JOSTLE_MOVEMENTS)
+            {
+                uint8_t j = depositor_data.jostle_counter;
+                float dtheta = depositor_data.jostle_routine[j];
+                bool stepper = stepper_commandAngle(STEPPER_DEPOSITOR, 
+                                         DEPOSITOR_ARM_CENTER_ANGLE + dtheta,
+                                         0.0,
+                                         DEPOSITOR_ARM_NAV_RATE,
+                                         DEPOSITOR_ARM_NAV_RATE);
+                if (stepper == false)
+                {
+                    // wait for current motion to finish
+                }
+                else
+                {
+                    depositor_data.jostle_counter++;
+                }
+            }
+            else
             {
                 if (servo_command(SERVO_DEPOSITOR,
                                   DEPOSITOR_OPEN_ANGLE,
                                   DEPOSITOR_OPEN_STEPS,
                                   DEPOSITOR_OPEN_RATE) == false)
                 {
-                    // do nothing
+                    // wait for cup to open
                 }
                 else
                 {
+                    // reset jostle counter for next time, change state
+                    depositor_data.jostle_counter = 0;
                     next_state = DEPOSITOR_STATE_ENTERING_IDLE;
                 }
             }
@@ -202,10 +249,6 @@ void depositor_init(void)
     servo_init(SERVO_DEPOSITOR, DEPOSITOR_CLOSE_ANGLE);
     switch_init(SWITCH_DEPOSITOR);
     stepper_init(STEPPER_DEPOSITOR);
-
-    // TODO remove this
-    stepper_init(STEPPER_BELT_TOP);
-    stepper_init(STEPPER_BELT_BOTTOM);
 }
 
 void depositor_run10ms(void)

fw/src/app/lighting.cpp

@@ -7,6 +7,7 @@
 #include "system_state.h"
 
 #include "dev/light.h"
+#include "dev/servo.h"
 
 /*******************************************************************************
 *                               C O N S T A N T S                              *
@@ -17,6 +18,8 @@
 #define LIGHTING_SIDELIGHT_HOMING_RATE 250
 #define LIGHTING_SIDELIGHT_IDLE_HEIGHT_STEPS 6000
 
+#define LIGHTING_NAV_RATE 1500
+
 #define LIGHTING_BACKLIGHT_ON  0
 #define LIGHTING_BACKLIGHT_OFF 65535
 #define LIGHTING_SIDELIGHT_ON  0
@@ -81,7 +84,7 @@ static lighting_state_E lighting_update_state(lighting_state_E curr_state)
             if (stepper_command(STEPPER_SIDELIGHT,
                                 LIGHTING_SIDELIGHT_IDLE_HEIGHT_STEPS,
                                 LIGHTING_SIDELIGHT_UP,
-                                750,
+                                LIGHTING_NAV_RATE,
                                 750,
                                 50) == false)
             {
@@ -122,7 +125,7 @@ static lighting_state_E lighting_update_state(lighting_state_E curr_state)
             if (stepper_command(STEPPER_SIDELIGHT, 
                                 LIGHTING_SIDELIGHT_IDLE_HEIGHT_STEPS, 
                                 LIGHTING_SIDELIGHT_DOWN, 
-                                750,
+                                LIGHTING_NAV_RATE,
                                 750,
                                 50) == false)
             {
@@ -170,6 +173,7 @@ void lighting_init(void)
 {
     PT_INIT(&lighting_data.thread);
 
+    servo_init(SERVO_COVER, 0);
     stepper_init(STEPPER_SIDELIGHT);
     switch_init(SWITCH_SIDELIGHT);
     light_init(LIGHT_BACK, LIGHTING_BACKLIGHT_OFF);

fw/src/app/motor_runner.cpp

@@ -28,7 +28,7 @@ typedef struct
 *          P R I V A T E    F U N C T I O N    D E C L A R A T I O N S         *
 *******************************************************************************/ 
 
-void run1ms(void);
+void run500us(void);
 
 /*******************************************************************************
 *                 S T A T I C    D A T A    D E F I N I T I O N S              *
@@ -44,10 +44,10 @@ static motor_runner_data_s motor_runner_data =
 *                      P R I V A T E    F U N C T I O N S                      *
 *******************************************************************************/ 
 
-static PT_THREAD(run1ms(struct pt* thread))
+static PT_THREAD(run500us(struct pt* thread))
 {
     PT_BEGIN(thread);
-    PT_WAIT_UNTIL(thread, scheduler_taskReleased(PERIOD_1ms, (uint8_t) MOTOR_RUNNER));
+    PT_WAIT_UNTIL(thread, scheduler_taskReleased(PERIOD_500us, (uint8_t) MOTOR_RUNNER));
 
     // call all the update functions of all motors
     for (uint8_t servo = 0; servo < SERVO_COUNT; servo++)
@@ -72,7 +72,7 @@ void motor_runner_init(void)
     PT_INIT(&motor_runner_data.thread);
 }
 
-void motor_runner_run1ms(void)
+void motor_runner_run500us(void)
 {
-    run1ms(&motor_runner_data.thread);
+    run500us(&motor_runner_data.thread);
 }

fw/src/app/motor_runner.h

@@ -21,6 +21,6 @@
 *******************************************************************************/ 
 
 void motor_runner_init(void);
-void motor_runner_run1ms(void);
+void motor_runner_run500us(void);
 
 #endif // DEV_MOTOR

fw/src/app/scheduler.cpp

@@ -9,7 +9,8 @@
 *                               C O N S T A N T S                              *
 *******************************************************************************/ 
 
-#define BASE_PERIOD_MICROSEC (100)
+#define BASE_PERIOD_MICROSEC (50)
+#define COUNT_100us          (100    / BASE_PERIOD_MICROSEC) 
 #define COUNT_500us          (500    / BASE_PERIOD_MICROSEC)
 #define COUNT_1ms            (1000   / BASE_PERIOD_MICROSEC)
 #define COUNT_10ms           (10000  / BASE_PERIOD_MICROSEC)
@@ -31,12 +32,16 @@ typedef struct
     HardwareTimer* timer;
     // private volatile data, only expose to scheduler task
     volatile uint16_t counter;
+
+    volatile bool task_100us_flag;
     volatile bool task_500us_flag;
+
     volatile bool task_1ms_flag;
     volatile bool task_10ms_flag;
     volatile bool task_100ms_flag;
     // public data
     struct pt thread;
+    bool mutexes_500us[TASK_500us_COUNT];
     bool mutexes_1ms[TASK_1ms_COUNT];
     bool mutexes_10ms[TASK_10ms_COUNT];
     bool mutexes_100ms[TASK_100ms_COUNT];
@@ -57,6 +62,10 @@ static void scheduler_ISR(void)
     if (scheduler_data.counter < COUNT_100ms)
     {
         scheduler_data.counter++;
+        if (scheduler_data.counter % COUNT_100us == 0)
+        {
+            scheduler_data.task_100us_flag = true;
+        }
         if (scheduler_data.counter % COUNT_500us == 0)
         {
             scheduler_data.task_500us_flag = true;
@@ -80,12 +89,18 @@ static void scheduler_ISR(void)
     }
 }
 
-static PT_THREAD(run500us(struct pt* thread))
+static PT_THREAD(run100us(struct pt* thread))
 {
     PT_BEGIN(thread);
-    PT_WAIT_UNTIL(thread, scheduler_data.task_500us_flag);
+    PT_WAIT_UNTIL(thread, scheduler_data.task_100us_flag);
     // scheduling code here
-    if (scheduler_data.task_1ms_flag)
+    if (scheduler_data.task_500us_flag)
+    {
+        scheduler_data.task_500us_flag = false;
+        (void) memset(&scheduler_data.mutexes_500us, 1, 
+                      sizeof(scheduler_data.mutexes_500us));
+    }
+    else if (scheduler_data.task_1ms_flag)
     {
         scheduler_data.task_1ms_flag = false;
         (void) memset(&scheduler_data.mutexes_1ms, 1, 
@@ -107,7 +122,7 @@ static PT_THREAD(run500us(struct pt* thread))
     {
         // do nothing
     }
-    scheduler_data.task_500us_flag = false;
+    scheduler_data.task_100us_flag = false;
     PT_END(thread);
 }
 
@@ -127,15 +142,20 @@ void scheduler_init(void)
     PT_INIT(&scheduler_data.thread);
 }
 
-void scheduler_run500us(void)
+void scheduler_run100us(void)
 {
-    run500us(&scheduler_data.thread);
+    run100us(&scheduler_data.thread);
 }
 
 bool scheduler_taskReleased(task_period_E period, uint8_t task_id)
 {
     bool ret = false;
-    if (period == PERIOD_1ms)
+    if (period == PERIOD_500us)
+    {
+        ret = scheduler_data.mutexes_500us[task_id];
+        scheduler_data.mutexes_500us[task_id] = false;
+    }
+    else if (period == PERIOD_1ms)
     {
         ret = scheduler_data.mutexes_1ms[task_id];
         scheduler_data.mutexes_1ms[task_id] = false;