Diff: main.cpp

Revision:

1:f3fe6d2b7639

Parent:

0:9167ae5d9927

Child:

2:7c6391c8ca71

--- a/main.cpp	Mon Oct 16 08:32:40 2017 +0000
+++ b/main.cpp	Mon Oct 16 09:43:57 2017 +0000
@@ -3,7 +3,6 @@
 #include "encoder.h"
 #include "math.h"
 
-
 MODSERIAL pc(USBTX,USBRX);
 PwmOut speed1(D5);
 DigitalOut dir1(D4);
@@ -16,16 +15,18 @@
 Ticker encoderreadout;
 Encoder motor1(PTD0,PTC4);
 
+float PwmPeriod = 0.0001f;
+
 double count = 0; //set the counts of the encoder
-double angle = 0;//set the angles
+volatile double angle = 0;//set the angles
 
-double setpoint = 100;//I am setting it to move through 180 degrees
-double Kp = 0.32;// you can set these constants however you like depending on trial & error
-double Ki = 0.1;
-double Kd = 0.3;
+double setpoint = 180;//I am setting it to move through 180 degrees
+double Kp = 0.15;// you can set these constants however you like depending on trial & error
+double Ki = 1;
+double Kd = 0.5;
 
 float last_error = 0;
-float error = 0;
+float error1 = 0;
 float changeError = 0;
 float totalError = 0;
 float pidTerm = 0;
@@ -33,44 +34,60 @@
 
 volatile double potvalue = 0.0;
 volatile double position = 0.0;
-void readpot(){
+/*void readpot()
+{
+    
+}
+*/
+void PIDcalculation()
+{
     potvalue = pot.read();
-    position = motor1.getPosition();
-    pc.printf("pos: %d, speed %f reference velocity = %.2f\r\n",motor1.getPosition(), motor1.getSpeed(), potvalue);
+    angle = motor1.getPosition()/4200.00*6.28;
     setpoint = potvalue*setpoint;
+    //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
     //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
-}
+    
+    if (angle <= setpoint) {
+        dir1 = 1;// Forward motion
+    } else {
+        dir1 = 0;//Reverse motion
+    }
     
-void PIDcalculation(){
-  
-  angle = (0.9 * position);//count to angle conversion
-  error = setpoint - angle;
-  
-  changeError = error - last_error; // derivative term
-  totalError += error; //accumalate errors to find integral term
-  pidTerm = (Kp * error) + (Ki * totalError) + (Kd * changeError);//total gain
-  pidTerm = constrain(pidTerm, -255, 255);//constraining to appropriate value
-  pidTerm_scaled = abs(pidTerm);//make sure it's a positive value
+    error1 = setpoint - angle;
+    if (error1 <= 0.2) {
+        speed1 = 0;
+    }
 
-  last_error = error;
+    changeError = error1 - last_error; // derivative term
+    totalError += error1; //accumalate errors to find integral term
+    pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
+    if (pidTerm >= 255) {
+        pidTerm = 255;
+    } else if (pidTerm <= -255) {
+        pidTerm = -255;
+    } else {
+        pidTerm = pidTerm;
+    }
+    //constraining to appropriate value
+    pidTerm_scaled = abs(pidTerm)/255;//make sure it's a positive value
+    last_error = error1;
 }
 
-void loop(){
-  
-  PIDcalculation();// find PID value
-  
-  if (angle < setpoint) {
-    dir1 = 1;// Forward motion
-  } else {
-    dir1 = 0;//Reverse motion
-  }
+int main()
+{
+    encoderreadout.attach(PIDcalculation,0.001f);
+    speed1.period(PwmPeriod);
+
+    while(true) {
+
 
-  speed1 = pidTerm_scaled;
+
+        speed1 = pidTerm_scaled;
 
-  pc.printf("WHEEL ANGLE:%d", angle);
+        pc.printf("WHEEL ANGLE:%d, PID_scaled = %f, error = %f\r", angle, pidTerm_scaled, error1);
 
-  delay(100);
+    }
+
 }
 
 
-  
\ No newline at end of file