Diff: main.cpp

Revision:

0:9167ae5d9927

Child:

1:f3fe6d2b7639

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 16 08:32:40 2017 +0000
@@ -0,0 +1,76 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
+#include "math.h"
+
+
+MODSERIAL pc(USBTX,USBRX);
+PwmOut speed1(D5);
+DigitalOut dir1(D4);
+DigitalOut led1(D8);
+DigitalOut led2(D11);
+AnalogIn pot(A1);
+DigitalIn button1(D13);
+DigitalIn button2(D12);
+Ticker potmeterreadout;
+Ticker encoderreadout;
+Encoder motor1(PTD0,PTC4);
+
+double count = 0; //set the counts of the encoder
+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;
+
+float last_error = 0;
+float error = 0;
+float changeError = 0;
+float totalError = 0;
+float pidTerm = 0;
+float pidTerm_scaled = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
+
+volatile double potvalue = 0.0;
+volatile double position = 0.0;
+void readpot(){
+    potvalue = pot.read();
+    position = motor1.getPosition();
+    pc.printf("pos: %d, speed %f reference velocity = %.2f\r\n",motor1.getPosition(), motor1.getSpeed(), potvalue);
+    setpoint = potvalue*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);
+}
+    
+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
+
+  last_error = error;
+}
+
+void loop(){
+  
+  PIDcalculation();// find PID value
+  
+  if (angle < setpoint) {
+    dir1 = 1;// Forward motion
+  } else {
+    dir1 = 0;//Reverse motion
+  }
+
+  speed1 = pidTerm_scaled;
+
+  pc.printf("WHEEL ANGLE:%d", angle);
+
+  delay(100);
+}
+
+
+  
\ No newline at end of file