Diff: main.cpp

Revision:

3:e888f52a46bc

Parent:

2:7c6391c8ca71

Child:

4:75f6e4845194

--- a/main.cpp	Mon Oct 16 10:15:23 2017 +0000
+++ b/main.cpp	Mon Oct 16 13:33:14 2017 +0000
@@ -2,7 +2,9 @@
 #include "MODSERIAL.h"
 #include "encoder.h"
 #include "math.h"
+#include "HIDScope.h"
 
+//HIDScope scope(1);
 MODSERIAL pc(USBTX,USBRX);
 PwmOut speed1(D5);
 DigitalOut dir1(D4);
@@ -13,6 +15,7 @@
 DigitalIn button2(D12);
 Ticker potmeterreadout;
 Ticker encoderreadout;
+Ticker Frequency;
 Encoder motor1(PTD0,PTC4);
 
 float PwmPeriod = 0.0001f;
@@ -20,9 +23,9 @@
 double count = 0; //set the counts of the encoder
 volatile double angle = 0;//set the angles
 
-double setpoint = 180;//I am setting it to move through 180 degrees
-double Kp = 0.216;// you can set these constants however you like depending on trial & error
-double Ki = 1.8;
+double setpoint = 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;
 
 float last_error = 0;
@@ -34,6 +37,9 @@
 
 volatile double potvalue = 0.0;
 volatile double position = 0.0;
+
+
+
 /*void readpot()
 {
 
@@ -43,43 +49,62 @@
 {
     potvalue = pot.read();
     angle = motor1.getPosition()/4200.00*6.28;
-    setpoint = potvalue*setpoint;
+    setpoint = potvalue*6.28f;
     //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) {
+
+
+    error1 = setpoint - angle;
+
+    changeError = (error1 - last_error)/0.001f; // derivative term
+    totalError += error1*0.001f; //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;
+    } else {
+        pidTerm = pidTerm;
+    }
+    //constraining to appropriate value
+        if (pidTerm >= 0) {
         dir1 = 1;// Forward motion
     } else {
         dir1 = 0;//Reverse motion
     }
-
-    error1 = setpoint - angle;
-
-    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;
+    pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
+    if(pidTerm_scaled >= 0.55f){
+        pidTerm_scaled = 0.55f;
     }
-    //constraining to appropriate value
-    pidTerm_scaled = abs(pidTerm)/255;//make sure it's a positive value
+        
     last_error = error1;
+    speed1 = pidTerm_scaled+0.45f;
 }
+/*
+void ReadOut()
+{
+   scope.set(0,angle);
+   scope.send(); 
+}
+*/
 
 int main()
 {
-    encoderreadout.attach(PIDcalculation,0.001f);
+    encoderreadout.attach(PIDcalculation,0.01f);
     speed1.period(PwmPeriod);
-
+    
+    int count = 0;
     while(true) {
-  
-        speed1 = pidTerm_scaled;
-        pc.printf("WHEEL ANGLE:%d, PID_scaled = %f, error = %f\r", angle, pidTerm_scaled, error1);
-
+        
+        count++;
+        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);
+        }
+        
+        wait(0.001f);
     }
 
 }