Diff: main.cpp

Revision:

0:9cf3d3dc621b

Child:

1:08d285e33eea

Child:

4:e8b3fba77dd0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 31 20:30:48 2017 +0000
@@ -0,0 +1,457 @@
+//#includes--------------------------------------------------------------------------------------------------------
+#include "mbed.h"
+#include "MODSERIAL.h" //for more efficient serial comm
+#include "FastPWM.h" //To fix a bug in the standard mbed lib concerning pwm
+
+//#efines--------------------------------------------------------------------------------------------------------
+#define PWMCYCLETIMEUS 50 //20 kHz PWM cycle time
+#define MAX_ENCODER_POS_RIGHT -1453 //Encoder counts at right end-stop
+#define MAX_ENCODER_POS_LEFT 1333 //Encoder counts at left end-stop
+#define LOOPDURATIONUS 200 //microsec
+#define LOOPDURATION ((float)LOOPDURATIONUS*0.000001f) //sec
+#define LEDPERIODS 1000 //How many loops before changing the LED state
+#define CALIBPWM 0.085f //12.5% of max PWM is the calibration PWM
+#define ENCODER_CPR 1024.0f //Counts per revolution for the encode (1x quad, to avoid missing counts)
+#define TWO_PI 6.283185307179586476925286766559f //rad
+#define WORKSPACEBOUND 7.0f //workspace bound for admittane model
+#define MAXVEL 100.0f //maximal admittance control velocity in rad/s
+#define FORCESENSORGAIN 15.134f //N per measured unit
+
+//Low pass filter filter coeffs, 2nd order, 200 Hz
+double b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929};
+double a[3] = {1.00000000000000, -1.822694925196308, 0.837181651256023};
+
+//Pins and interrupts--------------------------------------------------------------------------------------------------------
+// Remember that pins D4,D5,D6,D7 are internally connected to the drivers! Do not use D6 and D7.
+Ticker mainLoop; //Main loop runs at fixed fequency
+
+//Inputs
+AnalogIn    measuredForce(A5);  //Pin that measures analog force
+InterruptIn EncoderA(D2);       //Encoder interrupt for Channel A
+DigitalIn   EncoderB(D3);       //Encoder Channel B
+
+//Outputs
+DigitalOut  motorDir(D4);       //Motor Direction Pin
+FastPWM     motorPWM(D5);       //Motor PWM out pin
+
+//Communication for teleoperation
+//Ethernet eth;
+
+//For DBG
+DigitalOut  dbgPin(D1);         //Possible debug pin to check with scope
+MODSERIAL   pc(USBTX, USBRX);   //Serial communication with host PC
+DigitalOut  ledRed(LED_RED);    //LED RED
+DigitalOut  ledBlue(LED_BLUE);  //LED BLUE
+DigitalOut  ledGreen(LED_GREEN);//LED GREEN
+
+//Enums--------------------------------------------------------------------------------------------------------
+enum States {   stateIdle,                  stateCalibration,       stateHoming,            stateOPImpedance,
+                stateOPAdmittance,          stateOPTeleOpPosPos,    stateOPTeleOpImpedance,
+                stateOPTeleOpAdmittance,    stateFailed=99
+            }; //System states
+enum LedColors { colorCyan, colorMagenta, colorBlue, colorYellow, colorGreen, colorRed, colorBlack, colorWhite}; //Possible LED colors
+
+//Global variables--------------------------------------------------------------------------------------------------------
+//General
+long loopCounter = 0; //How many loops have passed. Will wrap around after 2^32-1
+int currentState = stateIdle; //Current state of state machine
+int statePrev = -1; //Previous state of state machine
+float stateTime = 0.0f; //How long have we been in this state?
+int ledCnt = 0; //Loops for LED state
+bool ledState = false; //Current LED state (on/off = true/false)
+Timer t; // Main timer to measure time
+
+//Controller references
+float refPos = 0.0f; //System reference position
+float refVel = 0.0f; //System reference velocity
+float refAcc = 0.0f; //System reference acceleration
+float u = 0.0f; //Control signal (PWM duty cycle)
+
+//Controller gains
+const float K_p = 4.5f; //Proportional position gain
+const float K_d = 0.03f; //Differential position gain
+const float K_i = 0.000f; //Integral position gain
+const float I_ff = 0.0000; //Feed-forward mass
+float posError = 0.0f;
+float velError = 0.0f;
+float integratedError = 0.0f;
+
+//Virtual model properties
+const float virtualMass = 0.0015f;
+const float virtualStiffness = 50.0f;
+const float virtualDamping = 3.0f;
+const float springPos = 0.0f;
+
+//Measured variables
+float motorPos = 0.0f;
+float motorPos_prev = 0.0f;
+float motorVel = 0.0f;
+float force = 0.0f;
+long encoderPos = 0; //Current encoder position
+long encoderPos_prev = 0; //Previous sample encoder position
+float encoderVel = 0.0f; // Estimated encoder velocity
+
+//DEbug and communication variables (unused?)
+char buffer[0x80]; //128 byte buffer (necessary?)
+
+//Functions--------------------------------------------------------------------------------------------------------
+//Encoder Interrupt Routine
+void encoderFunctionA()
+{
+    if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
+        encoderPos++;
+    } else {
+        encoderPos--;
+    }
+}
+
+//Velocity filtering
+double velocityFilter(float x)
+{
+    double y = 0.0; //Output
+    static double y_1 = 0.0; //Output last loop
+    static double y_2 = 0.0; //Output 2 loops ago
+    static double x_1 = 0.0; //Input last loop
+    static double x_2 = 0.0; //Input two loops ago
+
+    //Finite difference equation for 2nd order Butterworth low-pass
+    y = -a[1]*y_1 - a[2]*y_2 + x*b[0] + x_1*b[1] + x_2*b[2];
+    y_2 = y_1;
+    y_1 = y;
+    x_2 = x_1;
+    x_1 = x;
+    return (float)y;
+}
+
+//Limit a function, passed by reference
+void limit(float &x, float lower, float upper)
+{
+    if (x > upper)
+        x = upper;
+    if (x < lower)
+        x = lower;
+}
+
+//Set led colors
+void setLed(int ledColorArg)
+{
+    switch(ledColorArg) {
+        case colorCyan:
+            ledRed = 1;
+            ledGreen = 0;
+            ledBlue = 0;
+            break;
+        case colorMagenta:
+            ledRed = 0;
+            ledGreen = 1;
+            ledBlue = 0;
+            break;
+        case colorBlue:
+            ledRed = 1;
+            ledGreen = 1;
+            ledBlue = 0;
+            break;
+        case colorYellow:
+            ledRed = 0;
+            ledGreen = 0;
+            ledBlue = 1;
+            break;
+        case colorGreen:
+            ledRed = 1;
+            ledGreen = 0;
+            ledBlue = 1;
+            break;
+        case colorRed:
+            ledRed = 0;
+            ledGreen = 1;
+            ledBlue = 1;
+            break;
+        case colorBlack:
+            ledRed = 1;
+            ledGreen = 1;
+            ledBlue = 1;
+            break;
+        case colorWhite:
+            ledRed = 0;
+            ledGreen = 0;
+            ledBlue = 0;
+            break;
+    }
+}
+
+void setPWM(float u)
+{
+    //Possibly limit, if required
+    limit(u,-1.0f,1.0f);
+    //limit(u,-0.3f,0.3f); //DEBUG
+    //Set proper PWM
+    if (u >= 0.0f) {
+        motorPWM = u;
+        motorDir = false;
+    } else {
+        motorPWM = -u;
+        motorDir = true;
+    }
+}
+
+void doNetwork()
+{
+    //int size = eth.receive();
+    //if(size > 0)
+    //{
+        //eth.read(buf, size);
+        
+    //}
+}
+
+void mapIn()
+{
+    //Measure relevant signals
+    motorPos = (float)encoderPos / ENCODER_CPR * TWO_PI; //motor position in rad
+    encoderVel = (float)(encoderPos - encoderPos_prev) / LOOPDURATION; //encoder velocity in counts/s
+    encoderPos_prev = encoderPos; //encoder position in counts
+    motorVel = encoderVel / ENCODER_CPR * TWO_PI; //motor velocity in rad/s
+    motorVel = velocityFilter(motorVel); //filtered motor velocity in rad/s
+    motorPos_prev = motorPos; //Previous motor position in rad
+    force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force
+    doNetwork(); //Receive inputs over network (if available)
+}
+
+void mapOut()
+{
+    //Apply control
+    setPWM(u);
+    //setPWM(0); //DEBUG ONLY
+    //Send network packets if required
+}
+
+//State functions--------------------------------------------------------------------------------------------------------
+
+//During the wait/idle state
+inline void doStateIdle()
+{
+    //Control
+    u = 0.0f;
+    
+    //Handle LED
+    ledCnt++;
+    if (ledCnt > LEDPERIODS) {
+        ledState = !ledState;
+        if (ledState)
+            setLed(colorYellow);
+        else
+            setLed(colorBlack);
+        ledCnt = 0;
+    }
+    
+    //State Guard:
+    if (t.read() > 3.0f) {
+        currentState = stateCalibration;
+        stateTime = t.read();
+    }
+}
+
+inline void doStateCalib()
+{
+    //Control
+    //Send out calibration PWM
+    u = CALIBPWM;
+
+    //Handle LED
+    ledCnt++;
+    if (ledCnt > LEDPERIODS) {
+        ledState = !ledState;
+        if (ledState)
+            setLed(colorBlue);
+        else
+            setLed(colorBlack);
+        ledCnt = 0;
+    }
+
+    //State Guard
+    if ((abs(motorVel) < 0.001f) && (t.read()-stateTime > 1.0f)) {
+        currentState = stateHoming;
+        encoderPos = MAX_ENCODER_POS_LEFT; //When calibrated, set encoder pos to the known value
+        stateTime = t.read();
+        refPos = (float)MAX_ENCODER_POS_LEFT /  ENCODER_CPR * TWO_PI; //Start at the left calibration point
+    }
+}
+
+inline void doStateHoming()
+{
+    //Calculate a ramp to location 0 (center) with -2 rad/s
+    refVel = -2.00f;
+    if (refPos > 0.0f) {
+        refPos += refVel*LOOPDURATION;//refVel*LOOPDURATION;
+    }
+    else {
+        refPos = 0.0f;
+        refVel = 0.0f;
+    }
+    
+    //Control
+    posError = refPos - motorPos;
+    integratedError += posError*LOOPDURATION;
+    velError = refVel - motorVel;
+    u = K_p * posError + K_d * velError + 0*K_i*integratedError;
+
+    //Handle LED
+    ledCnt++;
+    if (ledCnt > LEDPERIODS) {
+        ledState = !ledState;
+        if (ledState)
+            setLed(colorGreen);
+        else
+            setLed(colorBlack);
+        ledCnt = 0;
+    }
+    
+    //State Guard
+    if ((abs(encoderPos) < 6) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
+        integratedError = 0.0f;
+        setLed(colorGreen);
+        stateTime = t.read();
+        currentState = stateOPAdmittance;//stateOPImpedance;
+        /*if (doAdmittance) {
+            currentState = stateOPAdmittance;
+        } else {
+            currentState = stateOPImpedance;
+        }*/
+    }
+}
+
+inline void doStateOPAdmittance()
+{
+    //Virtual Model
+    refAcc = force / virtualMass - virtualStiffness * (refPos - springPos) - virtualDamping * refVel;
+    refVel += refAcc * LOOPDURATION;
+    
+    if (refVel > MAXVEL) 
+    {
+        refVel = MAXVEL;
+        refAcc = 0.0f;
+    } else if (refVel < -MAXVEL) 
+    {
+        refVel = -MAXVEL;
+        refAcc = 0.0f;
+    }
+
+    refPos += refVel * LOOPDURATION;
+    if (refPos > WORKSPACEBOUND) 
+    {
+        refPos = WORKSPACEBOUND;
+        refVel = 0.0f;
+        refAcc = 0.0f;
+    } else if (refPos < -WORKSPACEBOUND) 
+    {
+        refPos = -WORKSPACEBOUND;
+        refVel = 0.0f;
+        refAcc = 0.0f;
+    }
+
+    //Controller
+    posError = refPos - motorPos;
+    velError = refVel - motorVel;
+    integratedError += posError*LOOPDURATION;
+    u = refAcc*I_ff + K_p * posError + K_d * velError  + 0*K_i * integratedError;
+
+    //StateGuard
+    //if...
+}
+
+inline void doStateOPImpedance()
+{
+    //Controller is the virtual model in OPEN LOOP impedance
+    u = - virtualStiffness * (motorPos - springPos) - virtualDamping * motorVel;
+    //StateGuard
+    //if...
+}
+
+inline void doStateFailed()
+{
+    //Control
+    u = 0.0f;
+    
+    //Handle LED
+    ledCnt++;
+    if (ledCnt > LEDPERIODS) {
+        ledState = !ledState;
+        if (ledState)
+            setLed(colorRed);
+        else
+            setLed(colorBlack);
+        ledCnt = 0;
+    }
+
+    //No state guard.
+}
+
+//Loop Function--------------------------------------------------------------------------------------------------------
+void loopFunction()
+{
+    //This code is run every loop
+    loopCounter++;
+
+    //Measure relevant inputs
+    mapIn();
+
+    //Set Defaults
+    u = 0.0f;
+
+    //Do State Machine
+    switch(currentState) {
+        case stateIdle:
+            doStateIdle();
+            break;
+        case stateCalibration:
+            doStateCalib();
+            break;
+        case stateHoming:
+            doStateHoming();
+            break;
+        case stateOPImpedance:
+            doStateOPImpedance();
+            break;
+        case stateOPAdmittance:
+            doStateOPAdmittance();
+            break;
+        case stateFailed: //fall through
+            doStateFailed();
+            break;
+    }
+
+    mapOut(); //Set all relevant output pins
+}
+
+//Main--------------------------------------------------------------------------------------------------------
+int main()
+{
+    setLed(colorBlack); //Set LED to black
+    pc.baud(115200); //set Serial comm
+    t.start();
+    
+    //Set all interrupt requests to 2nd place priority
+    for(int n = 0; n < 86; n++) {
+        NVIC_SetPriority((IRQn)n,1);
+    }
+    //Set out motor encoder interrupt to 1st place priority
+    NVIC_SetPriority(PORTB_IRQn,0);
+
+    
+    EncoderA.rise(&encoderFunctionA); //Attach interrupts only to rising edge encoder channel A
+    motorPWM.period_us(PWMCYCLETIMEUS); //50 us = 20 kHz
+    mainLoop.attach_us(&loopFunction,LOOPDURATIONUS); //200 us = 5 kHz
+    
+    //Infinite while loop
+    while (true) {
+        //Print relevant stuff to terminal here, do not print in the in the loopFunction ticker!!
+        if (currentState != statePrev) { //When state changes
+            pc.printf(">>>>>State Changed to State: %d\r\n",currentState);
+            statePrev = currentState; //Update the prev state var
+        }
+        wait(0.01);
+        float millisecondsPassed = (float)loopCounter*(((float)LOOPDURATIONUS) / 1000.0f);
+        pc.printf("t:%f;State:%d;EncPos:%d;MotPos:%f;MotVel:%f;Force:%f;RefPos:%f;PosErr:%f;MotPWM:%f\r\n",millisecondsPassed,currentState,encoderPos,motorPos,motorVel,force,refPos,posError,u);
+    }
+}
+//EOF--------------------------------------------------------------------------------------------------------
\ No newline at end of file