Diff: main.cpp

Revision:

18:5c4e27db4d9e

Parent:

6:fc46581fe3e0

Child:

19:8746a2c663f8

--- a/main.cpp	Mon Oct 23 07:48:56 2017 +0000
+++ b/main.cpp	Wed Nov 01 15:31:09 2017 +0000
@@ -6,6 +6,7 @@
 
 //HIDScope scope(1);
 MODSERIAL pc(USBTX,USBRX);
+AnalogOut servospeed();
 PwmOut speed1(D5);
 PwmOut speed2(D6);
 DigitalOut dir1(D4);
@@ -14,7 +15,7 @@
 DigitalOut led1(D8);
 DigitalOut led2(D11);
 AnalogIn pot(A2);
-AnalogIn pot2(A1);
+AnalogIn pot2(A3);
 Ticker mainticker;
 Encoder motor1(PTD0,PTC4);
 Encoder motor2(D12,D13);
@@ -27,11 +28,16 @@
 double count2 = 0; //set the counts of the encoder
 volatile double angle2 = 0;//set the angles
 
-double setpoint = 6.28;//I am setting it to move through 180 degrees
-double setpoint2 = 6.28;//I am setting it to move through 180 degrees
-double Kp = 250;// you can set these constants however you like depending on trial & error
-double Ki = 100;
-double Kd = 0;
+double setpoint = 0;//I am setting it to move through 180 degrees
+double setpoint2 = 0;//I am setting it to move through 180 degrees
+
+double Kp = 15.25;// you can set these constants however you like depending on trial & error
+double Ki = 0.0;
+double Kd = 0.0;
+
+double Kp2 = 30;
+double Ki2 = 30;
+double Kd2 = 2;
 
 float last_error = 0;
 float error1 = 0;
@@ -58,10 +64,10 @@
 {
    
     potvalue = pot.read();
-    setpoint = potvalue*6.28f;
+    setpoint = potvalue*6.28f*15.0f;
     
-    potvalue2 = pot2.read();
-    setpoint2 = potvalue2*6.28f;
+    //potvalue2 = pot2.read();
+    setpoint2 = potvalue*6.28f*15.0f;
    
 }
     
@@ -78,14 +84,15 @@
     error1 = setpoint - angle;
     error2 = setpoint2 - angle2;
     
-    changeError = (error1 - last_error)/0.001f; // derivative term
-    totalError += error1*0.001f; //accumalate errors to find integral term
+    changeError = (error1 - last_error)/0.002f; // derivative term
+    totalError += error1*0.002f; //accumalate errors to find integral term
     pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
     pidTerm = pidTerm;
-    if (pidTerm >= 1000) {
-        pidTerm = 1000;
-    } else if (pidTerm <= -1000) {
-        pidTerm = -1000;
+    
+    if (pidTerm >= 100) {
+        pidTerm = 100;
+    } else if (pidTerm <= -100) {
+        pidTerm = -100;
     } else {
         pidTerm = pidTerm;
     }
@@ -95,19 +102,19 @@
     } else {
         dir1 = 0;//Reverse motion
     }
-    pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
+    pidTerm_scaled = abs(pidTerm)/100.0f; //make sure it's a positive value
     if(pidTerm_scaled >= 0.55f){
         pidTerm_scaled = 0.55f;
     }
     
     changeError2 = (error2 - last_error2)/0.001f; // derivative term
     totalError2 += error2*0.001f; //accumalate errors to find integral term
-    pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
+    pidTerm2 = (Kp2 * error2) + (Ki2 * totalError2) + (Kd2 * changeError2);//total gain
     pidTerm2 = pidTerm2;
-    if (pidTerm2 >= 1000) {
-        pidTerm2 = 1000;
-    } else if (pidTerm2 <= -1000) {
-        pidTerm2 = -1000;
+    if (pidTerm2 >= 100) {
+        pidTerm2 = 100;
+    } else if (pidTerm2 <= -100) {
+        pidTerm2 = -100;
     } else {
         pidTerm2 = pidTerm2;
     }
@@ -117,20 +124,26 @@
     } else {
         dir2 = 0;//Reverse motion
     }
-    pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
+    pidTerm_scaled2 = abs(pidTerm2)/100.0f; //make sure it's a positive value
     if(pidTerm_scaled2 >= 0.55f){
         pidTerm_scaled2 = 0.55f;
+        
     }
         
     last_error = error1;
-    speed1 = pidTerm_scaled+0.45f;
     last_error2 = error2;
-    speed2 = pidTerm_scaled2+0.45f;
+    
+ 
+        speed1 = pidTerm_scaled+0.45f;
+   
+        speed2 = pidTerm_scaled2+0.45f;
+    
+    
 }
 
 int main()
 {
-    mainticker.attach(PIDcalculation,0.01f);
+    mainticker.attach(PIDcalculation,0.002f);
     speed1.period(PwmPeriod);
     speed2.period(PwmPeriod);
     
@@ -141,7 +154,7 @@
         if(count == 100){
             count = 0;
             pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
-            pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
+            //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
         }