Diff: main.cpp

Revision:

5:8ae6d935a16a

Parent:

4:be465e9a12cb

Child:

6:f58052f57505

--- a/main.cpp	Thu Sep 24 12:46:27 2015 +0000
+++ b/main.cpp	Fri Sep 25 13:11:43 2015 +0000
@@ -9,7 +9,7 @@
 // Defines
 //****************************************************************************/
 #define RATE  0.01
-#define Kc   -2.6
+#define Kc    0.1
 #define Ti    0.0
 #define Td    0.0
 
@@ -17,7 +17,7 @@
 // Globals
 //****************************************************************************/
 Serial pc(USBTX, USBRX);
-HIDScope    scope(2);        // Instantize a 2-channel HIDScope object
+HIDScope    scope(3);        // Instantize a 2-channel HIDScope object
 Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
 //--------
 // Motors 
@@ -44,10 +44,12 @@
 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;
 //Velocity to reach.
 int goal = 3000;
-float measure;
+float request;
 
 //****************************************************************************/
 // Prototypes
@@ -68,9 +70,9 @@
 
 void initializePidControllers(void){
 
-    leftController.setInputLimits(0.0, 10500.0);
+    leftController.setInputLimits(0.0, 30000.0);
     leftController.setOutputLimits(0.0, 1.0);
-    leftController.setBias(1.0);
+    leftController.setBias(0.0);
     leftController.setMode(AUTO_MODE);
 
 }
@@ -92,20 +94,23 @@
     
     //Run for 3 seconds.
     while (endTimer.read() < 100){
-        measure = pot1.read()*10500;
-        leftController.setSetPoint(measure);
+        request = pot1.read()*30000;
+        leftController.setSetPoint(request);
         leftPulses = leftQei.getPulses();
         leftVelocity = (leftPulses - leftPrevPulses) / RATE;
         leftPrevPulses = leftPulses;
         
-        scope.set(0, measure);
-        scope.set(1, leftVelocity);
+        
         
-        leftController.setProcessValue(-leftVelocity);
+        leftController.setProcessValue(leftVelocity);
         leftPwmDuty = leftController.compute();
         leftMotor = leftPwmDuty;
-       
-        pc.printf("leftpusles: %d, lefVelocity: %f, leftPwmDuty: %f \n\r",leftPulses,leftVelocity,leftPwmDuty);
+        
+        scope.set(0, request);
+        scope.set(1, leftPwmDuty);
+        scope.set(2, leftVelocity);
+        
+        pc.printf("request: %f, lefVelocity: %f, output: %f \n\r",request,leftVelocity,leftPwmDuty);
 
         //fprintf(fp, "%f,%f\n", leftVelocity, goal);
         wait(RATE);