The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
Diff: main.cpp
- Revision:
- 1:cc5636894b95
- Parent:
- 0:80a37292f6b2
- Child:
- 2:2bc519e14bae
diff -r 80a37292f6b2 -r cc5636894b95 main.cpp --- a/main.cpp Wed Mar 09 02:45:40 2016 +0000 +++ b/main.cpp Sun Mar 13 15:22:39 2016 +0000 @@ -1,4 +1,4 @@ -// Description: combines the functionality of both the PIcontrol program that controls a motor and the Pixy subsystem +// 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 "rtos.h" @@ -7,7 +7,7 @@ #include "Pixy.h" #define MAX_BLOCKS 1 -#define TARGET 12 //the code of our target, very important, should this be octal...? +#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 @@ -59,12 +59,10 @@ // left PWM0.period(pwmPeriod); PWM0.pulsewidth(0); - Thread::wait(500); // right PWM1.period(pwmPeriod); PWM1.pulsewidth(0); - Thread::wait(500); //mutex for the left and right motor setpoints setpoint_mutex = osMutexCreate(osMutex(setpoint_mutex)); @@ -72,9 +70,9 @@ Bumper.rise(&ExtCollisionISR); // Attach the address of the interrupt handler to the rising edge of Bumper - // Start execution of the threads: PiControlThread,ExtCollisionThread, and SensorControlThread: + // Start execution of the threads: MotorControlThread,ExtCollisionThread, and SensorControlThread: SensorControl = osThreadCreate(osThread(SensorControlThread), NULL); - if(MOTORS_ON) PiControl = osThreadCreate(osThread(PiControlThread), 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 @@ -94,7 +92,7 @@ SpiReset = 1; wait_us(10); SpiReset = 0; - idCode = DE0.write(0x8004); //changed from 8002 for one motor to 8004 for two + idCode = DE0.write(0x8004); // Read count/time for both motors pc.printf("\n\rDevice Id: %d", idCode); // motor Kp and Ki @@ -102,10 +100,10 @@ Ki = 0.0000000001; } - // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval - // in seconds between interrupts, and start interrupt generation: - if(MOTORS_ON) PeriodicInt.attach(&PiControllerISR, .001); - PeriodicInt2.attach(&SensorControllerISR, .0167); //trigger sensor thread 60 times/sec + // 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; @@ -114,11 +112,10 @@ osTimerStart(OneShot, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. - if(!MOTORS_ON){ - 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, 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); + /* if (!isMeasuring && (Setpoint!=0)) { pc.printf("\n\r-----------------------------------\n\r"); @@ -149,7 +146,7 @@ //find the target block in all of the blocks that were sent for(x=0;x<count;x++){ - if (pixy.blocks[x].signature == TARGET) break; + if (pixy.blocks[x].signature == TARGET_DECIMAL) break; if (x==count) pc.printf("no target in frame"); } @@ -169,7 +166,7 @@ speed_global = speed; } - // give setpoints to PiControlThread + // give setpoints to MotorControlThread osMutexWait(setpoint_mutex, osWaitForever); //enter mutex left_setpoint = speed - steer; right_setpoint = speed + steer; @@ -180,14 +177,14 @@ } // ******** Control Thread ******** -void PiControlThread(void const *argument) { +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 PiControlThread is signaled. + led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled. dP_L = DE0.write(0); // Read QEI-0 position register dt_L = DE0.write(0); // Read QEI-0 time register @@ -216,13 +213,14 @@ } // ******** 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 PiControllerISR(void) { - osSignalSet(PiControl,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. +void MotorControllerISR(void) { + osSignalSet(MotorControl,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt. } // ******** Period Timer Interrupt Handler ********