Diff: main.cpp

Revision:

2:646e3bca12a8

Parent:

1:ac598811dd00

--- a/main.cpp	Sat Nov 27 11:37:20 2010 +0000
+++ b/main.cpp	Mon Jun 27 23:12:28 2016 +0000
@@ -3,12 +3,13 @@
 //****************************************************************************/
 #include "PID.h"
 #include "QEI.h"
+#include "mbed.h"
 
 //****************************************************************************/
 // Defines
 //****************************************************************************/
-#define RATE  0.01
-#define Kc   -2.6
+#define RATE  0.1
+#define Kc   1.0
 #define Ti    0.0
 #define Td    0.0
 
@@ -16,19 +17,27 @@
 // Globals
 //****************************************************************************/
 //--------
-// Motors 
+// Motors
 //--------
 //Left motor.
-PwmOut leftMotor(p23);
-DigitalOut leftBrake(p19);
-DigitalOut leftDirection(p28);
-QEI leftQei(p29, p30, NC, 624);
+PwmOut leftMotor(PB_4);
+DigitalOut leftBrake(PA_15);
+DigitalOut leftDirection(PB_7);
+QEI leftQei(PC_10, PC_12, NC, 624);
 PID leftController(Kc, Ti, Td, RATE);
-//-------
-// Files
-//-------
-LocalFileSystem local("local");
-FILE* fp;
+
+//Right motor.
+PwmOut rightMotor(PB_10);
+DigitalOut rightBrake(PA_13);
+DigitalOut rightDirection(PA_14);
+QEI rightQei(PA_5, PA_6, NC, 624);
+PID rightController(Kc, Ti, Td, RATE);
+
+//--------
+// Serial
+//--------
+Serial pc(SERIAL_TX,SERIAL_RX);
+
 //--------
 // Timers
 //--------
@@ -40,8 +49,14 @@
 volatile int leftPrevPulses = 0;
 volatile float leftPwmDuty  = 1.0;
 volatile float leftVelocity = 0.0;
+
+volatile int rightPulses     = 0;
+volatile int rightPrevPulses = 0;
+volatile float rightPwmDuty  = 1.0;
+volatile float rightVelocity = 0.0;
+
 //Velocity to reach.
-int goal = 3000;
+int goal = 50;
 
 //****************************************************************************/
 // Prototypes
@@ -51,54 +66,140 @@
 //Set up PID controllers with appropriate limits and biases.
 void initializePidControllers(void);
 
-void initializeMotors(void){
+void initializeMotors(void)
+{
 
     leftMotor.period_us(50);
     leftMotor = 1.0;
     leftBrake = 0.0;
     leftDirection = 0;
 
+    rightMotor.period_us(50);
+    rightMotor = 1.0;
+    rightBrake = 0.0;
+    rightDirection = 0;
+
 }
 
-void initializePidControllers(void){
+void initializePidControllers(void)
+{
 
     leftController.setInputLimits(0.0, 10500.0);
     leftController.setOutputLimits(0.0, 1.0);
     leftController.setBias(1.0);
     leftController.setMode(AUTO_MODE);
 
+    rightController.setInputLimits(0.0, 10500.0);
+    rightController.setOutputLimits(0.0, 1.0);
+    rightController.setBias(1.0);
+    rightController.setMode(AUTO_MODE);
 }
 
-int main() {
+int main()
+{
 
+    int updateTime=0;
+
+    pc.baud(115200);
+ pc.printf("start\n\r");
     //Initialization.
     initializeMotors();
+    
+    wait(1);
+    /*
+    while(1){
+        leftPulses = leftQei.getPulses();
+        rightPulses = rightQei.getPulses();
+        pc.printf("leftPulses = %d\n\r",leftPulses);
+        pc.printf("rightPulses = %d\n\r",rightPulses);
+        wait(0.5);
+        }
+    */
     initializePidControllers();
 
-    //Open results file.
-    fp = fopen("/local/pidtest.csv", "w");
-    
+
     endTimer.start();
 
     //Set velocity set point.
     leftController.setSetPoint(goal);
+    rightController.setSetPoint(goal);
 
     //Run for 3 seconds.
-    while (endTimer.read() < 3.0){
-        leftPulses = leftQei.getPulses();
-        leftVelocity = (leftPulses - leftPrevPulses) / RATE;
-        leftPrevPulses = leftPulses;
-        leftController.setProcessValue(leftVelocity);
-        leftPwmDuty = leftController.compute();
-        leftMotor = leftPwmDuty;
-        fprintf(fp, "%f,%f\n", leftVelocity, goal);
-        wait(RATE);
+    while (endTimer.read() < 15.0f) {
+        
+        leftDirection = 1;
+        rightDirection = 1;
+        
+        if( (endTimer.read_ms()-updateTime) > (RATE*1000)) {
+            leftPulses = leftQei.getPulses();
+            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
+            leftPrevPulses = leftPulses;
+            leftController.setProcessValue(leftVelocity);
+            leftPwmDuty = leftController.compute();
+            leftMotor = leftPwmDuty;
+
+            rightPulses = rightQei.getPulses();
+            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
+            rightPrevPulses = rightPulses;
+            rightController.setProcessValue(rightVelocity);
+            rightPwmDuty = rightController.compute();
+            rightMotor = rightPwmDuty;
+
+            updateTime = endTimer.read_ms();
+
+            pc.printf("leftPulses = %d\n\r",leftPulses);
+            pc.printf("rightPulses = %d\n\r",rightPulses);
+            pc.printf("leftPwm = %f\n\r",leftPwmDuty);
+            pc.printf("rightPwm = %f\n\r",rightPwmDuty);
+        }
+        //wait(RATE);
     }
 
     //Stop motors.
     leftMotor  = 1.0;
+    rightMotor =1.0;
     
-    //Close results file.
-    fclose(fp);
+    rightDirection = 0;
+    leftDirection = 0;
+    endTimer.reset();
+    wait(1.0f);
+
+    pc.printf("stop\n]r");
+
+    //Set velocity set point.
+    leftController.setSetPoint(goal+1000);
+    rightController.setSetPoint(goal);
+
+        leftDirection = 1;
+        rightDirection = 1;
+
+    //Run for 2 seconds.
+    while (endTimer.read() < 2.0f) {
 
+        if(endTimer.read_ms()-updateTime > RATE*1000) {
+            leftPulses = leftQei.getPulses();
+            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
+            leftPrevPulses = leftPulses;
+            leftController.setProcessValue(leftVelocity);
+            leftPwmDuty = leftController.compute();
+            leftMotor = leftPwmDuty;
+
+            rightPulses = rightQei.getPulses();
+            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
+            rightPrevPulses = rightPulses;
+            rightController.setProcessValue(rightVelocity);
+            rightPwmDuty = rightController.compute();
+            rightMotor = rightPwmDuty;
+
+            updateTime = endTimer.read_ms();
+
+            pc.printf("leftPulses = %d\n\r",leftPulses);
+            pc.printf("rightPulses = %d\n\r",rightPulses);
+        }
+        //wait(RATE);
+    }
+    
+    rightDirection = 0;
+    leftDirection = 0;
+    
 }