Diff: main.cpp

Revision:

1:cc5636894b95

Parent:

0:80a37292f6b2

Child:

2:2bc519e14bae

--- 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 ********