Diff: robot.cpp

Revision:

3:dfb6733ae397

Parent:

2:2bc519e14bae

Child:

4:01252f56e0e5

--- a/robot.cpp	Sun Mar 13 17:55:42 2016 +0000
+++ b/robot.cpp	Mon Mar 14 00:40:28 2016 +0000
@@ -1,298 +1,191 @@
-// Description: combines the functionality of both the MotorControl program that controls a motor and the Pixy subsystem
-//              to create a program that will control the robot
-
 #include "robot.h"
 #include "global.h"
-#include "Pixy.h"
+
+// Global Definitions
+// ----------------------------------------------------------------
 
 #define MAX_BLOCKS 1
-#define TARGET_DECIMAL 0x10 //the code of our target in decimal
-
-//controls whether or not motors are a part of the program
-//for our test of the PIXY, they will not be
-#define MOTORS_ON 0
+#define TARGET_DECIMAL 10 
+#define PWM_PERIOD 0.001
 
-//steering inputs, may want to change to variables or something
-#define STEER_SETPOINT 160
-#define STEER_KP 1
-#define STEER_KI 0
-#define STEER_MAX 2147483648
+#define MOTOR_KP 0.000120
+#define MOTOR_KI 0.0000000001
 
-//speed inputs
-#define HEIGHT_SETPOINT 160
-#define SPEED_KP 1
-#define SPEED_KI 0
-#define SPEED_MAX 2147483648
+#define X_SETPOINT 160
+#define STEERING_KP 0.01
+#define STEERING_KI 0.005
 
-// Processes and threads
-int32_t SignalPi, SignalWdt, SignalExtCollision, SignalSensor;
-osThreadId MotorControl,SensorControl,WdtFault,ExtCollision;
-osThreadDef(MotorControlThread, osPriorityRealtime, DEFAULT_STACK_SIZE); // Declare Motor as a thread/process
-osThreadDef(ExtCollisionThread, osPriorityHigh, DEFAULT_STACK_SIZE);  // Declare External Collision as a thread/process
-osThreadDef(SensorControlThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE);  // Declare Sensor as a thread/process
-osTimerDef(Wdtimer, Watchdog); // Declare a watch dog timer
+#define HEIGHT_SETPOINT 100
+#define SPEED_KP 0.01
+#define SPEED_KI 0.01
+#define SPEED_MAX 10
 
-Ticker MotorPeriodicInt;      // Periodic interupt for motor thread   
-Ticker SensorPeriodicInt;    // Periodic interupt for sensor thread        
-InterruptIn Bumper(p8);  // External interrupt pin declared as Bumper
-
-//shared variables between sensor thread and motor thread, protected by mutex
-float left_setpoint, right_setpoint;
-
-//the setpoint mutex
-osMutexId setpoint_mutex;
-osMutexDef(setpoint_mutex);
+// Interrupt and thread control
+osThreadId motorId, navigationId, wdtId;
+osThreadDef(MotorThread, osPriorityRealtime, DEFAULT_STACK_SIZE); 
+osThreadDef(NavigationThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE); 
+osTimerDef(Wdtimer, WatchdogISR);
+int32_t motorSignal, navigationSignal;
+Ticker motorPeriodicInt;  
+Ticker sensorPeriodicInt;   
 
-//motorPI function variables
-float L_xState, R_xState; //the states for the left and right pi control loop (must have them outside of function to make them cumulative)
-
-//PI function variables
-float steer_xState, speed_xState; //the states for the steering and speed pi control loop
+// Mutex to protect left and right motor setpoints
+osMutexId motorMutex;
+osMutexDef(motorMutex);
 
-//main()variables
-short idCode;
-char isMeasuring;
-char inKey;
-float pwmPeriod = 0.001;
-
-//global variables
-float Kp, Ki; //kp and ki for motors
+// Set points and display variables
+float leftMotor, rightMotor;
+int xTracked, heightTracked;
+float speed, steering;
 
-// Various debugging variables
-float vels[60], times[60], errs[60], xvals[60], uvals[60]; // Used to measure step response
-int x_global, height_global;
-float speed_global, steer_global;
+// PI internal variables (TODO: move to object)
+float steeringIntegral, speedIntegral; 
+float leftMotorIntegral, rightMotorIntegral; 
 
-// ******** Main Thread ********
-int RUER() {  // This thread executes first upon reset or power-on.
-    
-    if(MOTORS_ON){
-        leftPwm.period(pwmPeriod);
-        leftPwm.pulsewidth(0);
 
-        rightPwm.period(pwmPeriod);
-        rightPwm.pulsewidth(0);
-   
-        //mutex for the left and right motor setpoints
-        setpoint_mutex = osMutexCreate(osMutex(setpoint_mutex));
-    }
+// Functions
+// ----------------------------------------------------------------
+void AutoTrack() { 
     
-    Bumper.rise(&ExtCollisionISR); // Attach the address of the interrupt handler to the rising edge of Bumper
+    leftPwm.period(PWM_PERIOD);
+    leftPwm.pulsewidth(0);
+    rightPwm.period(PWM_PERIOD);
+    rightPwm.pulsewidth(0);
+
+    pixy.setSerialOutput(&pc);
+    motorMutex = osMutexCreate(osMutex(motorMutex));
+
+    bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
     
-    // Start execution of the threads: MotorControlThread,ExtCollisionThread, and SensorControlThread:
-    SensorControl = osThreadCreate(osThread(SensorControlThread), NULL);
-    if(MOTORS_ON) MotorControl = osThreadCreate(osThread(MotorControlThread), NULL);
-    ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
-    
+    // Start execution of the threads MotorThread, and NavigationThread:
+    navigationId = osThreadCreate(osThread(NavigationThread), NULL);
+    motorId = osThreadCreate(osThread(MotorThread), NULL);
+ 
     // Start the watch dog timer and enable the watch dog interrupt
-    osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
-    led3=0; // Clear watch dog led3 status
+    osTimerId oneShotId = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
+    led3 = 0; // Clear watch dog led3 status
     
-    if(MOTORS_ON){
-        //SPI Section
-        pc.printf("\n\rStarting SPI...");
-        // Restart with a new SPI protocol.
-        deSpi.format(16,1); // 16 bit mode 1
-        ioReset = 0; 
-        ioReset = 1; 
-        wait_us(10);
-        ioReset = 0;
-        spiReset = 0; 
-        spiReset = 1; 
-        wait_us(10);
-        spiReset = 0; 
-        idCode = deSpi.write(0x8004); // Read count/time for both motors
-        pc.printf("\n\rDevice Id: %d", idCode);
+    //SPI Initialization
+    pc.printf("\n\rStarting SPI...");
+    deSpi.format(16,1); // 16 bit mode 1
+    ioReset = 0; ioReset = 1; 
+    wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; 
+    wait_us(10); spiReset = 0; 
+    pc.printf("\n\rDevice Id: %d", deSpi.write(0x8004)); // Read count & time for both motors
     
-        // motor Kp and Ki
-        Kp = 0.000120;
-        Ki = 0.0000000001;
-    }
-    
-    // Specify address of the ISRs and intervals in seconds between interrupts
+    // Specify periodic ISRs and their intervals in seconds
     // TODO: Optimize interrupt time for motor... Currently too fast
-    if(MOTORS_ON) MotorPeriodicInt.attach(&MotorControllerISR, .001);
-    SensorPeriodicInt.attach(&SensorControllerISR, .0167); //trigger sensor thread 60 times/sec
-    
-    //isMeasuring = 1; 
-    //Setpoint = 6; 
+    motorPeriodicInt.attach(&MotorISR, .001);
+    sensorPeriodicInt.attach(&NavigationISR, 0.1); //trigger sensor thread 60 times/sec
     
     while (1) {
         
-        osTimerStart(OneShot, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
+        osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
 
-        pc.printf("X Coordinate: %d, Steering: %d,", x_global, steer_global);
-        pc.printf("Height: %d, Speed: %d,", height_global, speed_global);
-        pc.printf(" L setpoint: %d, R setpoint: %d \n\r", left_setpoint, right_setpoint);
+        pc.printf("X Coordinate: %d, Height: %d \r\n", xTracked, heightTracked);
+        pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
+        pc.printf("Speed Int: %f, Steering Int: %f \r\n", speedIntegral, steeringIntegral);
+        pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n", leftMotor, rightMotor);
         
         Thread::wait(500); // Go to sleep for 500 ms
     }
 }
 
-// ******** Control Thread ********
-void SensorControlThread(void const *argument) {
-    float speed, steer;
-    int x, count, height;
-    
-    Pixy pixy(Pixy::SPI, p11, p12, p13);
-    
-    while (1) {
-        osSignalWait(SignalSensor, osWaitForever); //note: SignalSensor is just zero, waits for any signal dirrected at thread.
-
-        count = pixy.getBlocks(130); //get as many blocks as are available
-
-        //find the target block in all of the blocks that were sent
-        for(x=0;x<count;x++){
-            if (pixy.blocks[x].signature == TARGET_DECIMAL) break;
-            if (x==count) pc.printf("no target in frame");
-        }
-
-        // if blocks were returned
-        if (count){
-
-            //height = CalcHeight() //find the feedback from the pixy for the height
-            height = pixy.blocks[(x-1)].height; //use this for now
-
-            PI(&steer_xState, &steer, STEER_SETPOINT, Kp, Ki, pixy.blocks[(x-1)].x, STEER_MAX); //the feedback for the steering is the x location of the target block
-            PI(&speed_xState, &speed, HEIGHT_SETPOINT, Kp, Ki, height, SPEED_MAX);
-
-            if(!MOTORS_ON){
-                x_global = pixy.blocks[x-1].x;
-                height_global = height;
-                steer_global = steer;
-                speed_global = speed;
-            }
-
-            // give setpoints to MotorControlThread
-            osMutexWait(setpoint_mutex, osWaitForever); //enter mutex
-            left_setpoint = speed - steer;
-            right_setpoint = speed + steer;
-            osMutexRelease(setpoint_mutex); //exit mutex
-        }
-
-    } 
-}
-    
-// ******** Control Thread ********
-void MotorControlThread(void const *argument) {
-    short dP_L, dt_L, dP_R, dt_R;
-    float uL,uR;
-    float left_set,right_set;
-
-    while (1) {
-        osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received. SignalPi is just zero lol, waits for any signal to thread
-        led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
-        
-        dP_L = deSpi.write(0); // Read QEI-0 position register
-        dt_L = deSpi.write(0);  // Read QEI-0 time register
-        dP_R = deSpi.write(0); // Read QEI-0 position register
-        dt_R = deSpi.write(0);  // Read QEI-0 time register
-        
-        osMutexWait(setpoint_mutex, osWaitForever);//enter mutex
-        left_set= left_setpoint; //copy setpoint
-        right_set= right_setpoint;
-        osMutexRelease(setpoint_mutex);//exit mutex
-
-        uL = motorPI(0, left_set, Kp, Ki, dP_L, dt_L);
-        uR = motorPI(1, right_set, Kp, Ki, dP_R, dt_R);
-        
-        leftPwm.pulsewidth(fabs(uL));
-        rightPwm.pulsewidth(fabs(uR));
-    } 
-}
-
-// ******** Collision Thread ********
-void ExtCollisionThread(void const *argument) {
-    while (1) {
-        osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received
-        led4 = 1;
-    }
-}
-
-// ******** Watchdog Interrupt Handler ********
-// TODO: Shutdown system
-void Watchdog(void const *n) {
-    led3=1; // led3 is activated when the watchdog timer times out
-}
-
-// ******** Period Timer Interrupt Handler ********
-void MotorControllerISR(void) {
-    osSignalSet(MotorControl,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt.
-}
-
-// ******** Period Timer Interrupt Handler ********
-void SensorControllerISR(void) {
-    osSignalSet(SensorControl,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt. (basically sends signal to id of thread)
-}
-    
-// ******** Collision Interrupt Handler ********
-void ExtCollisionISR(void) {
-    osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt.
-}
-
-// motorPI - motor pi control extacted from previous work
-// Input:   0 for Left motor, 1 for right motor
-// Output: pwm pulsewidth for corresponding motor
-
-float motorPI(int RL_Motor, float Setpoint, float Kp, float Ki, short dP1, short dt1){
-    int dP32, i;
-    float angVel, e, u, xState;
-    
-    if (dP1 & 0x00008000) {
-        dP32 = dP1 | 0xFFFF0000;
-    } else {
-        dP32 = dP1;
-    }   
-        
-    angVel = QE2RadsPerSec(dP32, dt1);
-        
-    e = Setpoint - angVel;
-
-    RL_Motor ? xState = R_xState : xState = L_xState; //state is cumulative, select left or right state global variable
-        
-    // Avoid integrator wind up
-     if((u>=pwmPeriod)||(u<=-pwmPeriod)){
-        //xState= xState; - turns off integrator when u maxed
-    }else{
-        xState = xState + e;
-    }
-
-    RL_Motor ? R_xState = xState : L_xState = xState; //update state variable
-        
-    u = Ki*xState + Kp*e;
-        
-    // Set direction and pwm
-    if (u >= 0) {
-       RL_Motor ? rightDir = 1 : leftDir = 1;   
-    } else {
-       RL_Motor ? rightDir = 0 : leftDir = 0;
-    }
-        
-    if (isMeasuring) {
-        vels[i] = angVel;
-        times[i] = i;
-        errs[i] = e;
-        xvals[i] = xState;
-        uvals[i] = u;
-        if (i>=59) {
-            isMeasuring = 0;
-        }
-        i++;
-    }
-        
-    if (fabs(u) > pwmPeriod) u = pwmPeriod;
-    
-    return u;
-}
-
-void AutoTrack(void) {
-    
-}
-
 void ManualControl(void) {
 
 }
 
 void PixySubsystem(void) { 
 
+}
+
+
+// Threads
+// ----------------------------------------------------------------
+void NavigationThread(void const *argument) {
+    int count;
+    
+    while (1) {
+        osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR
+
+        count = pixy.getBlocks(1);
+        
+        // If target returned
+        if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
+            heightTracked = pixy.blocks[0].height; //use this for now
+            xTracked = pixy.blocks[0].x;
+
+            PI(HEIGHT_SETPOINT-heightTracked, &speed, &speedIntegral, SPEED_KP, SPEED_KI, SPEED_MAX);
+            PI(X_SETPOINT-xTracked, &steering, &steeringIntegral, STEERING_KP, STEERING_KI, 2*SPEED_MAX);
+
+            // Give setpoints to MotorThread
+            // Do not update speeds if already turning as fast as possible
+            osMutexWait(motorMutex, osWaitForever); 
+            leftMotor = speed - steering;
+            rightMotor = speed + steering;
+            osMutexRelease(motorMutex); 
+
+        }
+    } 
+}
+
+void MotorThread(void const *argument) {
+    float leftSet, rightSet, angVel;
+    short dP, dt;
+    float uL, uR;
+
+    while (1) {
+        osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
+        led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
+        
+        // Get setpoints from navigation
+        osMutexWait(motorMutex, osWaitForever); 
+        leftSet = leftMotor;
+        rightSet = rightMotor;
+        osMutexRelease(motorMutex); 
+        
+        // Run PI control on left motor
+        dP = deSpi.write(0);
+        dt = deSpi.write(0); 
+        angVel = QE2RadsPerSec(dP, dt);
+        PI(leftSet-angVel, &uL, &leftMotorIntegral, MOTOR_KP, MOTOR_KI, PWM_PERIOD);
+        
+        // Run PI control on right motor
+        dP = deSpi.write(0);
+        dt = deSpi.write(0); 
+        angVel = QE2RadsPerSec(dP, dt);
+        PI(rightSet-angVel, &uR, &rightMotorIntegral, MOTOR_KP, MOTOR_KI, PWM_PERIOD);
+        
+        // Output new PWM and direction (motors have opposite orientations)
+        if (uL >= 0) leftDir = 1;   
+        else leftDir = 0;
+        
+        if (uR >= 0) rightDir = 0;   
+        else rightDir = 1;
+        
+        leftPwm.pulsewidth(fabs(uL));
+        rightPwm.pulsewidth(fabs(uR));
+    } 
+}
+
+
+// Interrupt Service Routines
+// ----------------------------------------------------------------
+
+// TODO: Shutdown system
+void WatchdogISR(void const *n) {
+    led3=1; // Activated when the watchdog timer times out
+}
+
+void MotorISR(void) {
+    osSignalSet(motorId,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt
+}
+
+void NavigationISR(void) {
+    osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt
+}
+    
+// TODO: Shutdown system
+void CollisionISR(void) {
+    led4 = 1; // Activated on collision
 }
\ No newline at end of file