Diff: main.cpp

Revision:

9:07189a75e979

Parent:

8:55ca92c0e39d

Child:

12:d7bb475bb82d

--- a/main.cpp	Mon Oct 05 13:35:20 2015 +0000
+++ b/main.cpp	Tue Oct 06 10:17:13 2015 +0000
@@ -1,109 +1,69 @@
 //****************************************************************************/
 // Includes
-//****************************************************************************/
 #include "PID.h"
 #include "QEI.h"
-#include "HIDScope.h"        // Require the HIDScope library
-
-//****************************************************************************/
-// Defines
-//****************************************************************************/
-#define RATE  0.01 
-#define Kc    0.35
-#define Ti    0.15
-#define Td    0.0
+#include "HIDScope.h"
+#include "inits.h" // all globals, pin and variable initialization 
 
-//****************************************************************************/
-// Globals
-//****************************************************************************/
-Serial pc(USBTX, USBRX);
-HIDScope    scope(3);        // Instantize a 2-channel HIDScope object
-Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
-//--------
-// Motors 
-//--------
-//Left motor.
-PwmOut leftMotor(D5);
-DigitalOut leftBrake(D3);
-DigitalOut leftDirection(D4);
-QEI leftQei(D12, D13, NC, 624);
-PID leftController(Kc, Ti, Td, RATE);
-AnalogIn pot1(A0);
-AnalogIn pot2(A1);
-
-//--------
-// Timers
-//--------
-Timer endTimer;
-//--------------------
-// Working variables.
-//--------------------
-volatile int leftPulses     = 0;
-volatile int leftPrevPulses = 0;
-volatile float leftPwmDuty  = 1.0;
-volatile float leftPwmDutyPrev = 1.0;
-volatile float leftPwmDutyChange;
-volatile float leftVelocity = 0.0;
-
-float request;
-float request_pos;
-float request_neg;
-
+void setGoFlag(){
+    goFlag=true;
+}
 
 int main() {
-    //Initialization of motor
-    leftMotor.period_us(50);
-    leftMotor = 1.0;
-    leftBrake = 0.0;
-    leftDirection = 1;
     
-    //Initialization of PID controller
-    leftController.setInputLimits(-1.0, 1.0);
-    leftController.setOutputLimits(-1.0, 1.0);
-    leftController.setBias(0.0);
-    leftController.setMode(AUTO_MODE);
-
-
+    // initialize (defined in inits.h)
+    initMotors();
+    initPIDs();
+    
+    motorControlTicker.attach(&setGoFlag, RATE);
+    
     endTimer.start(); //Run for 100 seconds.
     while (endTimer.read() < 100){
-        // get 'emg' signal
-        request_pos = pot1.read();
-        request_neg = pot2.read();
-        
-        // determine if forward or backward signal is stronger and set reference
-        if (request_pos > request_neg){
-            request = request_pos; 
-        } 
-        else {
-            request = -request_neg;
-        }
-        leftController.setSetPoint(request);
+        if (goFlag==true){
+            // get 'emg' signal
+            request_pos = pot1.read();
+            request_neg = pot2.read();
+            
+            // determine if forward or backward signal is stronger and set reference
+            if (request_pos > request_neg){
+                request = request_pos; 
+            } 
+            else {
+                request = -request_neg;
+            }
+            leftController.setSetPoint(request);
+            
+            // Velocity calculation
+            leftPulses = leftQei.getPulses();
+            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
+            leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
+            leftPrevPulses = leftPulses;
+                    
+            // PID control
+            leftController.setProcessValue(leftVelocity);
+            leftPwmDuty = leftController.compute();
+            if (leftPwmDuty < 0){
+                leftDirection = 0;
+                leftMotor = -leftPwmDuty;
+            }
+            else {
+                leftDirection = 1;
+                leftMotor = leftPwmDuty;
+            }
+            
+            // testing the right motor
+            rightMotor = leftMotor;
+            rightDirection=leftDirection;
+            
+            // User feadback
+            scope.set(0, request);
+            scope.set(1, leftPwmDuty);
+            scope.set(2, leftVelocity);
+            scope.send();
+            pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
         
-        // Velocity calculation
-        leftPulses = leftQei.getPulses();
-        leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE);
-        leftPrevPulses = leftPulses;
-                
-        // PID control
-        leftController.setProcessValue(leftVelocity);
-        leftPwmDuty = leftController.compute();
-        if (leftPwmDuty < 0){
-            leftDirection = 0;
-            leftMotor = -leftPwmDuty;
+            goFlag=false;
         }
-        else {
-            leftDirection = 1;
-            leftMotor = leftPwmDuty;
-        }
-        
-        // User feadback
-        scope.set(0, request);
-        scope.set(1, leftPwmDuty);
-        scope.set(2, leftVelocity);
-        scope.send();
-        pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
-
-        wait(RATE);
     }
 
     //Stop motors.