Diff: main.cpp

Revision:

7:e14e28d8cae3

Parent:

6:f58052f57505

Child:

8:55ca92c0e39d

--- a/main.cpp	Fri Sep 25 13:41:59 2015 +0000
+++ b/main.cpp	Mon Oct 05 13:28:12 2015 +0000
@@ -3,14 +3,14 @@
 //****************************************************************************/
 #include "PID.h"
 #include "QEI.h"
- #include "HIDScope.h"        // Require the HIDScope library
+#include "HIDScope.h"        // Require the HIDScope library
 
 //****************************************************************************/
 // Defines
 //****************************************************************************/
-#define RATE  0.01
-#define Kc    0.2
-#define Ti    0.0
+#define RATE  0.01 
+#define Kc    0.35
+#define Ti    0.15
 #define Td    0.0
 
 //****************************************************************************/
@@ -29,11 +29,8 @@
 QEI leftQei(D12, D13, NC, 624);
 PID leftController(Kc, Ti, Td, RATE);
 AnalogIn pot1(A0);
-//-------
-// Files
-//-------
-//LocalFileSystem local("local");
-//FILE* fp;
+AnalogIn pot2(A1);
+
 //--------
 // Timers
 //--------
@@ -48,40 +45,24 @@
 volatile float leftPwmDutyChange;
 volatile float leftVelocity = 0.0;
 //Velocity to reach.
-int goal = 3000;
 float request;
+float request_pos;
+float request_neg;
 
-//****************************************************************************/
-// Prototypes
-//****************************************************************************/
-//Set motors to go "forward", brake off, not moving.
-void initializeMotors(void);
-//Set up PID controllers with appropriate limits and biases.
-void initializePidControllers(void);
 
-void initializeMotors(void){
-
+int main() {
+    //Initialization of motor
     leftMotor.period_us(50);
     leftMotor = 1.0;
     leftBrake = 0.0;
     leftDirection = 1;
-
-}
-
-void initializePidControllers(void){
-
-    leftController.setInputLimits(0.0, 30000.0);
-    leftController.setOutputLimits(0.0, 1.0);
+    
+    //Initialization of PID controller
+    leftController.setInputLimits(-1.0, 1.0);
+    leftController.setOutputLimits(-1.0, 1.0);
     leftController.setBias(0.0);
     leftController.setMode(AUTO_MODE);
 
-}
-
-int main() {
-    //scopeTimer.attach_us(&scope, &HIDScope::send, 1e4);
-    //Initialization.
-    initializeMotors();
-    initializePidControllers();
 
     //Open results file.
     //fp = fopen("/local/pidtest.csv", "w");
@@ -94,23 +75,39 @@
     
     //Run for 3 seconds.
     while (endTimer.read() < 100){
-        request = pot1.read()*30000;
+        request_pos = pot1.read();
+        request_neg = pot2.read();
+        
+        if (request_pos > request_neg){
+            request = request_pos; 
+        } 
+        else {
+            request = -request_neg;
+        }
+        // Velocity calculation
         leftController.setSetPoint(request);
         leftPulses = leftQei.getPulses();
-        leftVelocity = (leftPulses - leftPrevPulses) / RATE;
+        leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE);
         leftPrevPulses = leftPulses;
         
         
-        
+        // PID control
         leftController.setProcessValue(leftVelocity);
         leftPwmDuty = leftController.compute();
-        leftMotor = leftPwmDuty;
+        if (leftPwmDuty < 0){
+            leftDirection = 0;
+            leftMotor = -leftPwmDuty;
+        }
+        else {
+            leftDirection = 1;
+            leftMotor = leftPwmDuty;
+        }
         
         scope.set(0, request);
         scope.set(1, leftPwmDuty);
         scope.set(2, leftVelocity);
         scope.send();
-        pc.printf("request: %f, lefVelocity: %f, output: %f \n\r",request,leftVelocity,leftPwmDuty);
+        pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
 
         //fprintf(fp, "%f,%f\n", leftVelocity, goal);
         wait(RATE);