Diff: robot.cpp

Revision:

2:2bc519e14bae

Child:

3:dfb6733ae397

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Sun Mar 13 17:55:42 2016 +0000
@@ -0,0 +1,298 @@
+// 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"
+
+#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
+
+//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
+
+//speed inputs
+#define HEIGHT_SETPOINT 160
+#define SPEED_KP 1
+#define SPEED_KI 0
+#define SPEED_MAX 2147483648
+
+// 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
+
+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);
+
+//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
+
+//main()variables
+short idCode;
+char isMeasuring;
+char inKey;
+float pwmPeriod = 0.001;
+
+//global variables
+float Kp, Ki; //kp and ki for motors
+
+// 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;
+
+// ******** 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));
+    }
+    
+    Bumper.rise(&ExtCollisionISR); // Attach the address of the interrupt handler to the 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 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
+    
+    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);
+    
+        // motor Kp and Ki
+        Kp = 0.000120;
+        Ki = 0.0000000001;
+    }
+    
+    // Specify address of the ISRs and intervals in seconds between interrupts
+    // 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; 
+    
+    while (1) {
+        
+        osTimerStart(OneShot, 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);
+        
+        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) { 
+
+}
\ No newline at end of file