Diff: main.cpp

Revision:

0:30871514c229

Child:

1:9149cfedd4d5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 12 21:07:30 2018 +0000
@@ -0,0 +1,151 @@
+//Works at speed 2.0, KPS 2.0e-2, KD 1.0e-4
+
+//Has tolerance check, Steering doesn't
+//Values from Simulation
+//#define KPS 0.0896852742245504f
+//#define KD 0.072870569295611f
+//#define KPS 0.03f
+
+#include "mbed.h"
+#include <iostream>
+
+#define TI 0.001f
+
+#define SCALAR 0.6f
+#define MINM 0.0f
+#define MAXM 1.0f
+#define KPM 0.1414f
+#define KI 19.7408f
+
+#define KPS 2.0E-2f //Original 2.0e-2
+#define KD 1.0e-4f
+#define SET 0.0f
+#define MINS 0.05f
+#define MAXS 0.1f
+#define BIAS 0.075f
+#define TOL 0.02f
+#define STEER_FREQ 0.02f //50 Hz
+#define STEERUPDATEPERIOD 0.05
+
+AnalogIn _right(PTB1); //Right sensor
+AnalogIn _left(PTB3); //Left sensor
+AnalogIn speed(PTB2);   //tachometer
+
+PwmOut servoSig(PTA13); //PWM output to control servo
+PwmOut gateDrive(PTA4);
+
+Serial bt(PTE0, PTE1); //COM12
+DigitalOut myled(LED_BLUE);
+
+Ticker control;
+Timer ctrlTimer;
+float data[6];
+
+float Setpoint = 0.0f;
+float errSum = 0.0;
+
+float fbPrev = 0.0f;
+
+void serCb()
+{
+    char x = bt.getc();
+    if (x == 'u')
+    {
+        Setpoint = Setpoint + 0.05;
+    }
+    else if(x == 'h')
+    {
+        Setpoint = Setpoint - 0.05;
+    }
+    else
+    {
+        bt.putc(x);
+    }
+    if(Setpoint > MAXM) Setpoint = MAXM;
+    if(Setpoint < MINM) Setpoint = MINM;
+    bt.printf("%f\r\n", Setpoint);     
+}
+
+void stop()
+{
+  Setpoint = 0.0f;  
+}
+
+void steer()
+{
+    float L = _left.read();
+    float R = _right.read();
+    if(L == 0.0f && R == 0.0f)
+    {
+        stop();
+    }    
+    float fb = L - R; 
+    float e = SET - fb;
+    float Controlleroutput = KPS * e - (KD * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
+    if (Controlleroutput > MAXS) Controlleroutput = MAXS;
+    else if (Controlleroutput < MINS) Controlleroutput = MINS;
+    if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD) 
+    {
+        ctrlTimer.reset();
+        servoSig.write(Controlleroutput);
+    }
+    fbPrev = fb;
+    data[0] = _right.read();
+    data[1] = _left.read();
+    data[2] = Controlleroutput;
+    data[3] = e;
+    
+}
+void drive()
+{
+    float error = Setpoint-speed.read();
+    errSum +=(error * TI);
+    float iTerm = KI*errSum;
+    if(iTerm > MAXM) iTerm = MAXM;
+    if(iTerm < MINM) iTerm = MINM; 
+    float output = KPM*error + iTerm;
+    if(output > MAXM) output = MAXM;
+    if(output < MINM) output = MINM;    
+    if(output < MINM) 
+    {
+        gateDrive.write(MINM);
+        //brake.write(1);
+    }
+    else 
+    {
+        //brake.write(0);
+        gateDrive.write(output);
+    }     
+    data[4] = gateDrive.read();
+    data[5] = speed.read(); 
+}
+
+void cb()
+{
+    steer();
+    drive();
+}
+
+int main()
+{   
+    //startup checks
+    bt.attach(&serCb);
+    servoSig.period(STEER_FREQ);
+    gateDrive.period(.00005f);
+    gateDrive.write(Setpoint);
+
+    ctrlTimer.start();
+    control.attach(&cb, TI);
+    
+    bt.baud(115200);
+    bt.printf("Right Left SteerControl SteerError GateDrive Speed\r\n");
+    while(1) {
+        myled = !myled;
+        bt.printf("%f ",data[0]);
+        bt.printf("%f ", data[1]);
+        bt.printf("%f ", data[2]);
+        bt.printf("%f ", data[3]);
+        bt.printf("%f ", data[4]);
+        bt.printf("%f\r\n", data[5]);
+    }
+}
\ No newline at end of file