Revision 19:05b8123905fb, committed 2017-04-24

Comitter:

balsamfir

Date:

Mon Apr 24 21:37:50 2017 +0000

Parent:

18:501f1007a572

Commit message:

Commit before share

Changed in this revision

--- a/PeriodicPI.h	Thu Apr 07 13:19:29 2016 +0000
+++ b/PeriodicPI.h	Mon Apr 24 21:37:50 2017 +0000
@@ -3,8 +3,6 @@
 
 #include "mbed.h"
 
-
-
 class PeriodicPI
 {
 public:

--- a/global.cpp	Thu Apr 07 13:19:29 2016 +0000
+++ b/global.cpp	Mon Apr 24 21:37:50 2017 +0000
@@ -5,16 +5,16 @@
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-DigitalOut leftDir(p24); //
+DigitalOut leftDir(p23); //
 DigitalOut rightDir(p22); //
-DigitalOut spiReset(p8); //changed
-DigitalOut ioReset(p14); //
+DigitalOut spiReset(p14); //changed
+DigitalOut ioReset(p15); //
 
 // Comunication 
 SPI deSpi(p5, p6, p7);
 Pixy pixy(Pixy::SPI, p11, p12, p13);
-//Serial pc(USBTX, USBRX); // PC serial channel
-Serial pc(p28, p27); // Bluetooth serial channel
+Serial pc(USBTX, USBRX); // PC serial channel
+//Serial pc(p9, p10); // Bluetooth serial channel
 
 // Control
 PeriodicPI leftMotorPI(MOTOR_PERIOD, MOTOR_KP, MOTOR_KI);
@@ -23,18 +23,15 @@
 PeriodicPI xPI(NAVIGATION_PERIOD, STEERING_KP, STEERING_KI);
 
 // Other
-PwmOut leftPwm(p23); //
-PwmOut rightPwm(p21); //
-InterruptIn bumper(p25);
+PwmOut leftPwm(p24); 
+PwmOut rightPwm(p21); 
 
-// Converts measurements from the QE2 to rads/sec
-float QE2RadsPerSec(short counts, short time) {    
-    return ((float)counts*122.62)/time;
+// Converts measurements from the FPGA to rads/sec
+float RadsPerSec(short counts, short time) {    
+    return ((float)counts*26.558)/time;
 }
 
 // Returns the last character
-char flushBuffer(void) { 
-    char ch;
-    while (pc.readable()) pc.putc(pc.getc());
-    return ch; 
+void FlushBuffer(void) { 
+    while (pc.readable()) pc.getc();
 }
\ No newline at end of file

--- a/global.h	Thu Apr 07 13:19:29 2016 +0000
+++ b/global.h	Mon Apr 24 21:37:50 2017 +0000
@@ -10,27 +10,29 @@
 
 // Preprocessor Definitions
 // ----------------------------------------------------------------
-#define SPEED_MAX 40 // rads/s
+#define SPEED_MAX 2 // rads/s
 #define MAX_BLOCKS 1
 #define X_SETPOINT 160
-#define HEIGHT_SETPOINT 90
-#define TARGET_DECIMAL 10 
+#define HEIGHT_SETPOINT 120
+#define TARGET_DECIMAL 1 
+
+// #define DEBUG 1
 
 #define PWM_PERIOD 0.001
-#define MOTOR_PERIOD 0.001
+#define MOTOR_PERIOD 0.01
 #define NAVIGATION_PERIOD 0.0167 // 60 times/sec
 
-#define MOTOR_KP 0.000120
-#define MOTOR_KI 0.0000001
+#define MOTOR_KP 0.0001 // 0.0001
+#define MOTOR_KI 0.0005 // 0.0001 //0.00001 //0.0000001
 
 #define STEERING_KP 0.10
 #define STEERING_KI 0
 
-#define SPEED_KP 0.6
+#define SPEED_KP 0.1
 #define SPEED_KI 0
 
 #define MOTOR_SAMPLES 300
-#define NAVIGATION_SAMPLES 240
+#define NAVIGATION_SAMPLES 200
 
 
 // Global variables
@@ -59,8 +61,7 @@
 // Other
 extern PwmOut leftPwm; 
 extern PwmOut rightPwm; 
-extern InterruptIn bumper;  // External interrupt pin declared as Bumper
 
 // Method prototypes
-char flushBuffer(void);
-float QE2RadsPerSec(short counts, short time);
\ No newline at end of file
+void FlushBuffer(void);
+float RadsPerSec(short counts, short time);
\ No newline at end of file

--- a/main.cpp	Thu Apr 07 13:19:29 2016 +0000
+++ b/main.cpp	Mon Apr 24 21:37:50 2017 +0000
@@ -4,25 +4,7 @@
 #include "global.h"
 #include "robot.h"
 
-// Definitions 
-// ----------------------------------------------------------------
-enum Mode {
-    AUTO_TRACK = '0', 
-    MANUAL_CONTROL = '1', 
-    SYSTEM_TUNING = '2'
-};
-
-void PrintMenu(Serial *pc);
-
-
-// Wiring - TODO
-// ----------------------------------------------------------------
-//
-//
-//
-//
-//
-//
+void PrintMenu(void);
 
 // Main Program
 // ----------------------------------------------------------------
@@ -31,14 +13,14 @@
     InitRobot();
 
     while (1) {
-        PrintMenu(&pc);
+        PrintMenu();
         mode = pc.getc();
         pc.printf("\r\n\r\n");
-        if ((mode == '0')||(mode == 'a')) {
+        if (mode == 'a') {
             AutoTrack();
-        } else if ((mode == '1')||(mode == 'm')) {
+        } else if (mode == 'm') {
             ManualControl();
-        } else if (mode == '2') {
+        } else if (mode == 't') {
             Tunning();
         } else {
             pc.printf("Error: Invalid Selection \r\n\r\n");
@@ -49,15 +31,15 @@
 
 // Other Functions
 // ----------------------------------------------------------------
-void PrintMenu(Serial *pc){
-    pc->printf("\e[1;1H\e[2J");
-    pc->printf("Select Mode: \r\n\r\n");
+void PrintMenu(){
+    pc.printf("\e[1;1H\e[2J");
+    pc.printf("Select Mode: \r\n\r\n");
     
-    pc->printf("---------------------------------------------------------------- \r\n");
-    pc->printf("0. Automated Tracking \r\n");
-    pc->printf("1. Manual Control \r\n");
-     pc->printf("2. System Tuning \r\n");
-    pc->printf("---------------------------------------------------------------- \r\n\r\n");
+    pc.printf("---------------------------------------------------------------- \r\n");
+    pc.printf("a. Automated Tracking \r\n");
+    pc.printf("m. Manual Control \r\n");
+    pc.printf("t. System Tuning \r\n");
+    pc.printf("---------------------------------------------------------------- \r\n\r\n");
     
-    pc->printf("=> ");
+    pc.printf("=> ");
 }

--- a/robot.cpp	Thu Apr 07 13:19:29 2016 +0000
+++ b/robot.cpp	Mon Apr 24 21:37:50 2017 +0000
@@ -59,8 +59,6 @@
     rightPwm.pulsewidth(0);
 
     motorMutex = osMutexCreate(osMutex(motorMutex));
-
-    bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
     
     // Start execution of the threads MotorThread, and NavigationThread:
     navigationId = osThreadCreate(osThread(NavigationThread), NULL);
@@ -73,15 +71,17 @@
     //SPI Initialization
     pc.printf("\n\rStarting SPI...");
     deSpi.format(16,1); // 16 bit mode 1
+    deSpi.frequency(500000);
     ioReset = 0; ioReset = 1; 
     wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; 
     wait_us(10); spiReset = 0; 
     pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors
+    FlushBuffer();
 }
 
+// Setup robot for auto tracking and listen for commands
 void AutoTrack(void) {
     char key;
-    bool isReady;
     motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
     sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
     
@@ -92,26 +92,21 @@
             if (key == 'q') {
                 ShutDown();
                 return;
-            } else if ((key == '1')||(key == 'm')) {
+            } else if (key == 'm') {
                 ShutDown();
                 ManualControl();
-            } else if (key == '~') {
-                isReady = true;   
-            }
+            } 
         }
         
-        if (isReady) {
-            pc.printf("X: %d, Height: %d \r\n", x, height);
-            pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering);
-            isReady = false;  
-            pc.putc('~');
-        }
+        pc.printf("X: %d, Height: %d \r\n", x, height);
+        pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering);
+
         Thread::wait(250);
     }
 }
 
+// Setup robot for manual control and listen for commands
 void ManualControl(void) {
-    bool isReady;
     char key;
     motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
     float speedRate, steeringRate;
@@ -123,25 +118,25 @@
             if (key == 'q') {
                 ShutDown();
                 return;
-            } else if ((key == '0')||(key == 'a')) {
+            } else if (key == 'a') {
                 ShutDown();
                 AutoTrack();
-            } else if (key == '~') {
-                isReady = true;   
+            } else if (key == '8') {
+                speedRate = (speedRate < 1)? speedRate + 0.1 : 1;
+            } else if (key == '5') {
+                speedRate = (speedRate > -1)? speedRate - 0.1 : -1;
+            } else if (key == '6') {
+                steeringRate = (steeringRate < 1)? steeringRate + 0.1 : 1;
+            } else if (key == '4') {
+                steeringRate = (steeringRate > -1)? steeringRate - 0.1 : -1;
             } 
+            
+            pc.printf("%.2f,%.2f\r\n", speedRate, steeringRate); 
         }
         
-        if (isReady) {
-            pc.scanf("%f :: %f", &speedRate, &steeringRate); 
-            speed = SPEED_MAX * speedRate;
-            steering = (SPEED_MAX * steeringRate);    
-            isReady = false;
-            pc.putc('~'); 
-        }
-        
-        osMutexWait(motorMutex, osWaitForever); 
-        leftMotor = speed - steering;
-        rightMotor = speed + steering;
+        osMutexWait(motorMutex, osWaitForever);  
+        leftMotor = SPEED_MAX*(speedRate + steeringRate);
+        rightMotor = SPEED_MAX*(speedRate - steeringRate);
         osMutexRelease(motorMutex); 
     }
 }
@@ -264,13 +259,13 @@
     leftMotorPI.Reset();
     rightMotorPI.Reset();
     pc.printf("Robot Shutdown... \r\n\r\n");
+    FlushBuffer();
 }
 
 // Threads
 // ----------------------------------------------------------------
 void NavigationThread(void const *argument) {
     int count;
-    int missingCount;
     
     while (1) {
         osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR
@@ -279,8 +274,7 @@
         
         // If target returned
         if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
-            missingCount = 0;
-            height = pixy.blocks[0].height; //use this for now
+            height = pixy.blocks[0].height;
             x = pixy.blocks[0].x;
             
             speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX);
@@ -288,25 +282,10 @@
 
             // Give setpoints to MotorThread
             osMutexWait(motorMutex, osWaitForever); 
-            if (speed >= 0) {
-                leftMotor = speed - steering;
-                rightMotor = speed + steering;
-            } else {
-                leftMotor = speed - steering;
-                rightMotor = speed + steering;
-            }
+            leftMotor = speed - steering;
+            rightMotor = speed + steering;
             osMutexRelease(motorMutex); 
-        } else {
-            // When the robot can't see the target spin in last known direction
-            if (missingCount >= 30) {
-                osMutexWait(motorMutex, osWaitForever);
-                leftMotor = (leftMotor > rightMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
-                rightMotor = (rightMotor > leftMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
-                osMutexRelease(motorMutex); 
-            } else {
-                missingCount++;    
-            }
-        }
+        } 
         
         if (isTunning && (navigationSample < NAVIGATION_SAMPLES)) {
             speedResponse[navigationSample] = speed; 
@@ -319,11 +298,11 @@
 void MotorThread(void const *argument) {
     float leftSet, rightSet, angVel;
     float timeOnLeft, timeOnRight;
+    short loop;
     short dP, dt;
 
     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); 
@@ -331,21 +310,22 @@
         rightSet = rightMotor;
         osMutexRelease(motorMutex); 
         
+        // Run PI control on right motor
+        dP = deSpi.write(0);
+        dt = deSpi.write(0); 
+        angVel = RadsPerSec(dP, dt); // motors have opposite orientations
+        timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD/2);
+        
         // Run PI control on left motor
         dP = deSpi.write(0);
         dt = deSpi.write(0); 
-        angVel = QE2RadsPerSec(dP, dt);
-        timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD);
+        angVel = -RadsPerSec(dP, dt);
+        timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD/2);
         
-        // Run PI control on right motor
-        dP = deSpi.write(0);
-        dt = deSpi.write(0); 
-        angVel = -QE2RadsPerSec(dP, dt); // motors have opposite orientations
-        timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD); //
         
         // Output new PWM and direction 
-        if (timeOnLeft >= 0) leftDir = 1;   
-        else leftDir = 0;
+        if (timeOnLeft >= 0) leftDir = 0;   
+        else leftDir = 1;
         
         if (timeOnRight >= 0) rightDir = 0;   
         else rightDir = 1;
@@ -358,6 +338,10 @@
             rightMotorResponse[motorSample] = timeOnRight;
             motorSample++;
         } 
+        
+        #ifdef DEBUG
+        loop = (loop < 1/MOTOR_PERIOD)? loop + 1 : 0;       
+        #endif
     } 
 }
 
@@ -365,9 +349,9 @@
 // Interrupt Service Routines
 // ----------------------------------------------------------------
 
-// TODO: Shutdown system
 void WatchdogISR(void const *n) {
     led3=1; // Activated when the watchdog timer times out
+    ShutDown();
 }
 
 void MotorISR(void) {
@@ -376,10 +360,4 @@
 
 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