Revision 0:8bf709d3440b, committed 2018-04-18

Comitter:

KDrainEE

Date:

Wed Apr 18 14:58:02 2018 +0000

Child:

1:cbd32e9800fb

Child:

2:22159ccbf771

Child:

3:eb933ff5ccaf

Commit message:

Attempting to fix motor oscillation

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 18 14:58:02 2018 +0000
@@ -0,0 +1,244 @@
+//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>
+#include <cmath>
+
+using std::pow;
+
+#define TI 0.001f
+
+#define SCALAR 0.53f
+#define MINM 0.0f
+#define MAXM 0.53f
+#define KPM 0.15f //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);
+DigitalOut brake(PTD0);
+
+Ticker control;
+Timer ctrlTimer;
+
+InterruptIn navRt(PTD2);
+InterruptIn navLft(PTD3);
+
+volatile int rightCount;
+volatile int leftCount;
+
+float data[6];
+
+float Setpoint = 0.0;
+float spHolder;
+float errSum = 0.0;
+float Kpm = 0.141;
+
+
+float fbPrev = 0.0f;
+float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
+float Kd = 1.0e-4;
+
+
+
+inline void applyBrake()
+{  
+    spHolder = Setpoint;
+    brake.write(1);
+    Setpoint = 0.0;
+}
+
+inline void releaseBrake()
+{
+    brake.write(0);
+    Setpoint = spHolder;
+}
+
+void display()
+{
+    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
+}
+
+void serCb()
+{
+    char x = bt.getc();
+    if (x == 'u')
+    {
+        Setpoint += 0.025;
+        display();
+    }
+    else if(x == 'h')
+    {
+        Setpoint -= 0.025;
+        display();
+    }
+    else if (x == 'i')
+    {
+        Kps += 1.0e-3;
+        display();
+    }
+    else if (x == 'j')
+    {
+        Kps -= 1.0e-3;
+        display();
+    }
+    else if (x == 'o')
+    {
+        Kd += 1.0e-5;
+        display();
+    }
+    else if (x == 'k')
+    {
+        Kd -= 1.0e-5;
+        display();
+    }
+    else if (x == 'b')
+    {
+        applyBrake();
+        display();
+       
+    }
+    else if (x == 'n')
+    {
+        releaseBrake();
+        display();
+    }
+    else if (x == 'p')
+    {
+        display();
+    }
+    else if (x == 'y')
+    {
+        Kpm += 0.003;
+        display();   
+    }   
+    else if(x == 'g')
+    {
+        Kpm -= 0.003; 
+        display();   
+    } 
+    else if (x == 'z')
+    {
+        bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
+    }
+    else
+    {
+        bt.printf("Invalid input");
+    }
+    if(Setpoint > MAXM) Setpoint = MAXM;
+    if(Setpoint < MINM) Setpoint = MINM;    
+}
+
+
+void incL()
+{      
+    leftCount++;
+}
+
+void incR()
+{
+    rightCount++;
+}
+
+void controller()
+{
+    //steering
+    float L = _left.read();
+    float R = _right.read();
+    float velocity = speed.read();
+//    if (L == 0.0 && R == 0.0)
+//    {
+//        //off track
+//        Setpoint = 0.0;
+//    }           
+    float fb = L - R; 
+    float e = SET - fb;
+    Kps = 0.001/Setpoint;
+    if (Kps > 0.03f) Kps = 0.03f;
+    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] = L;
+    data[1] = R;
+    data[2] = Controlleroutput;
+    data[3] = e;
+    
+    //driving
+    float error = Setpoint-velocity;
+    errSum +=(error * TI);
+
+    float Ki = 19.7408;
+    if (Setpoint < 0.15){
+        Ki = 4.9352;
+    } else if(Setpoint >= 0.15 && Setpoint < 0.3){
+        Ki = 9.8704;
+    }     
+    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;        
+
+    gateDrive.write(output);
+    data[4] = gateDrive.read();
+    data[5] = speed.read();    
+}
+
+int main()
+{   
+    //startup checks
+    bt.baud(115200);
+    bt.attach(&serCb);    
+    servoSig.period(STEER_FREQ);
+    gateDrive.period(.00005f);
+    gateDrive.write(Setpoint);
+
+    ctrlTimer.start();
+    control.attach(&controller, TI);
+    
+    rightCount = 0;
+    leftCount = 0; 
+
+    navRt.fall(&incR);
+    navLft.fall(&incL);
+    
+    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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 18 14:58:02 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file