Arvid Keemink / Mbed 2 deprecated BMHaptics1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM MODSERIAL mbed QEI

main.cpp@0:9cf3d3dc621b, 2017-05-31 (annotated)

Committer:
ArvidKeemink
Date:
Wed May 31 20:30:48 2017 +0000
Revision:
0:9cf3d3dc621b
Child:
1:08d285e33eea
Child:
4:e8b3fba77dd0
Student Version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ArvidKeemink 0:9cf3d3dc621b 1 //#includes--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 2 #include "mbed.h"
ArvidKeemink 0:9cf3d3dc621b 3 #include "MODSERIAL.h" //for more efficient serial comm
ArvidKeemink 0:9cf3d3dc621b 4 #include "FastPWM.h" //To fix a bug in the standard mbed lib concerning pwm
ArvidKeemink 0:9cf3d3dc621b 5
ArvidKeemink 0:9cf3d3dc621b 6 //#efines--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 7 #define PWMCYCLETIMEUS 50 //20 kHz PWM cycle time
ArvidKeemink 0:9cf3d3dc621b 8 #define MAX_ENCODER_POS_RIGHT -1453 //Encoder counts at right end-stop
ArvidKeemink 0:9cf3d3dc621b 9 #define MAX_ENCODER_POS_LEFT 1333 //Encoder counts at left end-stop
ArvidKeemink 0:9cf3d3dc621b 10 #define LOOPDURATIONUS 200 //microsec
ArvidKeemink 0:9cf3d3dc621b 11 #define LOOPDURATION ((float)LOOPDURATIONUS*0.000001f) //sec
ArvidKeemink 0:9cf3d3dc621b 12 #define LEDPERIODS 1000 //How many loops before changing the LED state
ArvidKeemink 0:9cf3d3dc621b 13 #define CALIBPWM 0.085f //12.5% of max PWM is the calibration PWM
ArvidKeemink 0:9cf3d3dc621b 14 #define ENCODER_CPR 1024.0f //Counts per revolution for the encode (1x quad, to avoid missing counts)
ArvidKeemink 0:9cf3d3dc621b 15 #define TWO_PI 6.283185307179586476925286766559f //rad
ArvidKeemink 0:9cf3d3dc621b 16 #define WORKSPACEBOUND 7.0f //workspace bound for admittane model
ArvidKeemink 0:9cf3d3dc621b 17 #define MAXVEL 100.0f //maximal admittance control velocity in rad/s
ArvidKeemink 0:9cf3d3dc621b 18 #define FORCESENSORGAIN 15.134f //N per measured unit
ArvidKeemink 0:9cf3d3dc621b 19
ArvidKeemink 0:9cf3d3dc621b 20 //Low pass filter filter coeffs, 2nd order, 200 Hz
ArvidKeemink 0:9cf3d3dc621b 21 double b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929};
ArvidKeemink 0:9cf3d3dc621b 22 double a[3] = {1.00000000000000, -1.822694925196308, 0.837181651256023};
ArvidKeemink 0:9cf3d3dc621b 23
ArvidKeemink 0:9cf3d3dc621b 24 //Pins and interrupts--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 25 // Remember that pins D4,D5,D6,D7 are internally connected to the drivers! Do not use D6 and D7.
ArvidKeemink 0:9cf3d3dc621b 26 Ticker mainLoop; //Main loop runs at fixed fequency
ArvidKeemink 0:9cf3d3dc621b 27
ArvidKeemink 0:9cf3d3dc621b 28 //Inputs
ArvidKeemink 0:9cf3d3dc621b 29 AnalogIn measuredForce(A5); //Pin that measures analog force
ArvidKeemink 0:9cf3d3dc621b 30 InterruptIn EncoderA(D2); //Encoder interrupt for Channel A
ArvidKeemink 0:9cf3d3dc621b 31 DigitalIn EncoderB(D3); //Encoder Channel B
ArvidKeemink 0:9cf3d3dc621b 32
ArvidKeemink 0:9cf3d3dc621b 33 //Outputs
ArvidKeemink 0:9cf3d3dc621b 34 DigitalOut motorDir(D4); //Motor Direction Pin
ArvidKeemink 0:9cf3d3dc621b 35 FastPWM motorPWM(D5); //Motor PWM out pin
ArvidKeemink 0:9cf3d3dc621b 36
ArvidKeemink 0:9cf3d3dc621b 37 //Communication for teleoperation
ArvidKeemink 0:9cf3d3dc621b 38 //Ethernet eth;
ArvidKeemink 0:9cf3d3dc621b 39
ArvidKeemink 0:9cf3d3dc621b 40 //For DBG
ArvidKeemink 0:9cf3d3dc621b 41 DigitalOut dbgPin(D1); //Possible debug pin to check with scope
ArvidKeemink 0:9cf3d3dc621b 42 MODSERIAL pc(USBTX, USBRX); //Serial communication with host PC
ArvidKeemink 0:9cf3d3dc621b 43 DigitalOut ledRed(LED_RED); //LED RED
ArvidKeemink 0:9cf3d3dc621b 44 DigitalOut ledBlue(LED_BLUE); //LED BLUE
ArvidKeemink 0:9cf3d3dc621b 45 DigitalOut ledGreen(LED_GREEN);//LED GREEN
ArvidKeemink 0:9cf3d3dc621b 46
ArvidKeemink 0:9cf3d3dc621b 47 //Enums--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 48 enum States { stateIdle, stateCalibration, stateHoming, stateOPImpedance,
ArvidKeemink 0:9cf3d3dc621b 49 stateOPAdmittance, stateOPTeleOpPosPos, stateOPTeleOpImpedance,
ArvidKeemink 0:9cf3d3dc621b 50 stateOPTeleOpAdmittance, stateFailed=99
ArvidKeemink 0:9cf3d3dc621b 51 }; //System states
ArvidKeemink 0:9cf3d3dc621b 52 enum LedColors { colorCyan, colorMagenta, colorBlue, colorYellow, colorGreen, colorRed, colorBlack, colorWhite}; //Possible LED colors
ArvidKeemink 0:9cf3d3dc621b 53
ArvidKeemink 0:9cf3d3dc621b 54 //Global variables--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 55 //General
ArvidKeemink 0:9cf3d3dc621b 56 long loopCounter = 0; //How many loops have passed. Will wrap around after 2^32-1
ArvidKeemink 0:9cf3d3dc621b 57 int currentState = stateIdle; //Current state of state machine
ArvidKeemink 0:9cf3d3dc621b 58 int statePrev = -1; //Previous state of state machine
ArvidKeemink 0:9cf3d3dc621b 59 float stateTime = 0.0f; //How long have we been in this state?
ArvidKeemink 0:9cf3d3dc621b 60 int ledCnt = 0; //Loops for LED state
ArvidKeemink 0:9cf3d3dc621b 61 bool ledState = false; //Current LED state (on/off = true/false)
ArvidKeemink 0:9cf3d3dc621b 62 Timer t; // Main timer to measure time
ArvidKeemink 0:9cf3d3dc621b 63
ArvidKeemink 0:9cf3d3dc621b 64 //Controller references
ArvidKeemink 0:9cf3d3dc621b 65 float refPos = 0.0f; //System reference position
ArvidKeemink 0:9cf3d3dc621b 66 float refVel = 0.0f; //System reference velocity
ArvidKeemink 0:9cf3d3dc621b 67 float refAcc = 0.0f; //System reference acceleration
ArvidKeemink 0:9cf3d3dc621b 68 float u = 0.0f; //Control signal (PWM duty cycle)
ArvidKeemink 0:9cf3d3dc621b 69
ArvidKeemink 0:9cf3d3dc621b 70 //Controller gains
ArvidKeemink 0:9cf3d3dc621b 71 const float K_p = 4.5f; //Proportional position gain
ArvidKeemink 0:9cf3d3dc621b 72 const float K_d = 0.03f; //Differential position gain
ArvidKeemink 0:9cf3d3dc621b 73 const float K_i = 0.000f; //Integral position gain
ArvidKeemink 0:9cf3d3dc621b 74 const float I_ff = 0.0000; //Feed-forward mass
ArvidKeemink 0:9cf3d3dc621b 75 float posError = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 76 float velError = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 77 float integratedError = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 78
ArvidKeemink 0:9cf3d3dc621b 79 //Virtual model properties
ArvidKeemink 0:9cf3d3dc621b 80 const float virtualMass = 0.0015f;
ArvidKeemink 0:9cf3d3dc621b 81 const float virtualStiffness = 50.0f;
ArvidKeemink 0:9cf3d3dc621b 82 const float virtualDamping = 3.0f;
ArvidKeemink 0:9cf3d3dc621b 83 const float springPos = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 84
ArvidKeemink 0:9cf3d3dc621b 85 //Measured variables
ArvidKeemink 0:9cf3d3dc621b 86 float motorPos = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 87 float motorPos_prev = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 88 float motorVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 89 float force = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 90 long encoderPos = 0; //Current encoder position
ArvidKeemink 0:9cf3d3dc621b 91 long encoderPos_prev = 0; //Previous sample encoder position
ArvidKeemink 0:9cf3d3dc621b 92 float encoderVel = 0.0f; // Estimated encoder velocity
ArvidKeemink 0:9cf3d3dc621b 93
ArvidKeemink 0:9cf3d3dc621b 94 //DEbug and communication variables (unused?)
ArvidKeemink 0:9cf3d3dc621b 95 char buffer[0x80]; //128 byte buffer (necessary?)
ArvidKeemink 0:9cf3d3dc621b 96
ArvidKeemink 0:9cf3d3dc621b 97 //Functions--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 98 //Encoder Interrupt Routine
ArvidKeemink 0:9cf3d3dc621b 99 void encoderFunctionA()
ArvidKeemink 0:9cf3d3dc621b 100 {
ArvidKeemink 0:9cf3d3dc621b 101 if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
ArvidKeemink 0:9cf3d3dc621b 102 encoderPos++;
ArvidKeemink 0:9cf3d3dc621b 103 } else {
ArvidKeemink 0:9cf3d3dc621b 104 encoderPos--;
ArvidKeemink 0:9cf3d3dc621b 105 }
ArvidKeemink 0:9cf3d3dc621b 106 }
ArvidKeemink 0:9cf3d3dc621b 107
ArvidKeemink 0:9cf3d3dc621b 108 //Velocity filtering
ArvidKeemink 0:9cf3d3dc621b 109 double velocityFilter(float x)
ArvidKeemink 0:9cf3d3dc621b 110 {
ArvidKeemink 0:9cf3d3dc621b 111 double y = 0.0; //Output
ArvidKeemink 0:9cf3d3dc621b 112 static double y_1 = 0.0; //Output last loop
ArvidKeemink 0:9cf3d3dc621b 113 static double y_2 = 0.0; //Output 2 loops ago
ArvidKeemink 0:9cf3d3dc621b 114 static double x_1 = 0.0; //Input last loop
ArvidKeemink 0:9cf3d3dc621b 115 static double x_2 = 0.0; //Input two loops ago
ArvidKeemink 0:9cf3d3dc621b 116
ArvidKeemink 0:9cf3d3dc621b 117 //Finite difference equation for 2nd order Butterworth low-pass
ArvidKeemink 0:9cf3d3dc621b 118 y = -a[1]*y_1 - a[2]*y_2 + x*b[0] + x_1*b[1] + x_2*b[2];
ArvidKeemink 0:9cf3d3dc621b 119 y_2 = y_1;
ArvidKeemink 0:9cf3d3dc621b 120 y_1 = y;
ArvidKeemink 0:9cf3d3dc621b 121 x_2 = x_1;
ArvidKeemink 0:9cf3d3dc621b 122 x_1 = x;
ArvidKeemink 0:9cf3d3dc621b 123 return (float)y;
ArvidKeemink 0:9cf3d3dc621b 124 }
ArvidKeemink 0:9cf3d3dc621b 125
ArvidKeemink 0:9cf3d3dc621b 126 //Limit a function, passed by reference
ArvidKeemink 0:9cf3d3dc621b 127 void limit(float &x, float lower, float upper)
ArvidKeemink 0:9cf3d3dc621b 128 {
ArvidKeemink 0:9cf3d3dc621b 129 if (x > upper)
ArvidKeemink 0:9cf3d3dc621b 130 x = upper;
ArvidKeemink 0:9cf3d3dc621b 131 if (x < lower)
ArvidKeemink 0:9cf3d3dc621b 132 x = lower;
ArvidKeemink 0:9cf3d3dc621b 133 }
ArvidKeemink 0:9cf3d3dc621b 134
ArvidKeemink 0:9cf3d3dc621b 135 //Set led colors
ArvidKeemink 0:9cf3d3dc621b 136 void setLed(int ledColorArg)
ArvidKeemink 0:9cf3d3dc621b 137 {
ArvidKeemink 0:9cf3d3dc621b 138 switch(ledColorArg) {
ArvidKeemink 0:9cf3d3dc621b 139 case colorCyan:
ArvidKeemink 0:9cf3d3dc621b 140 ledRed = 1;
ArvidKeemink 0:9cf3d3dc621b 141 ledGreen = 0;
ArvidKeemink 0:9cf3d3dc621b 142 ledBlue = 0;
ArvidKeemink 0:9cf3d3dc621b 143 break;
ArvidKeemink 0:9cf3d3dc621b 144 case colorMagenta:
ArvidKeemink 0:9cf3d3dc621b 145 ledRed = 0;
ArvidKeemink 0:9cf3d3dc621b 146 ledGreen = 1;
ArvidKeemink 0:9cf3d3dc621b 147 ledBlue = 0;
ArvidKeemink 0:9cf3d3dc621b 148 break;
ArvidKeemink 0:9cf3d3dc621b 149 case colorBlue:
ArvidKeemink 0:9cf3d3dc621b 150 ledRed = 1;
ArvidKeemink 0:9cf3d3dc621b 151 ledGreen = 1;
ArvidKeemink 0:9cf3d3dc621b 152 ledBlue = 0;
ArvidKeemink 0:9cf3d3dc621b 153 break;
ArvidKeemink 0:9cf3d3dc621b 154 case colorYellow:
ArvidKeemink 0:9cf3d3dc621b 155 ledRed = 0;
ArvidKeemink 0:9cf3d3dc621b 156 ledGreen = 0;
ArvidKeemink 0:9cf3d3dc621b 157 ledBlue = 1;
ArvidKeemink 0:9cf3d3dc621b 158 break;
ArvidKeemink 0:9cf3d3dc621b 159 case colorGreen:
ArvidKeemink 0:9cf3d3dc621b 160 ledRed = 1;
ArvidKeemink 0:9cf3d3dc621b 161 ledGreen = 0;
ArvidKeemink 0:9cf3d3dc621b 162 ledBlue = 1;
ArvidKeemink 0:9cf3d3dc621b 163 break;
ArvidKeemink 0:9cf3d3dc621b 164 case colorRed:
ArvidKeemink 0:9cf3d3dc621b 165 ledRed = 0;
ArvidKeemink 0:9cf3d3dc621b 166 ledGreen = 1;
ArvidKeemink 0:9cf3d3dc621b 167 ledBlue = 1;
ArvidKeemink 0:9cf3d3dc621b 168 break;
ArvidKeemink 0:9cf3d3dc621b 169 case colorBlack:
ArvidKeemink 0:9cf3d3dc621b 170 ledRed = 1;
ArvidKeemink 0:9cf3d3dc621b 171 ledGreen = 1;
ArvidKeemink 0:9cf3d3dc621b 172 ledBlue = 1;
ArvidKeemink 0:9cf3d3dc621b 173 break;
ArvidKeemink 0:9cf3d3dc621b 174 case colorWhite:
ArvidKeemink 0:9cf3d3dc621b 175 ledRed = 0;
ArvidKeemink 0:9cf3d3dc621b 176 ledGreen = 0;
ArvidKeemink 0:9cf3d3dc621b 177 ledBlue = 0;
ArvidKeemink 0:9cf3d3dc621b 178 break;
ArvidKeemink 0:9cf3d3dc621b 179 }
ArvidKeemink 0:9cf3d3dc621b 180 }
ArvidKeemink 0:9cf3d3dc621b 181
ArvidKeemink 0:9cf3d3dc621b 182 void setPWM(float u)
ArvidKeemink 0:9cf3d3dc621b 183 {
ArvidKeemink 0:9cf3d3dc621b 184 //Possibly limit, if required
ArvidKeemink 0:9cf3d3dc621b 185 limit(u,-1.0f,1.0f);
ArvidKeemink 0:9cf3d3dc621b 186 //limit(u,-0.3f,0.3f); //DEBUG
ArvidKeemink 0:9cf3d3dc621b 187 //Set proper PWM
ArvidKeemink 0:9cf3d3dc621b 188 if (u >= 0.0f) {
ArvidKeemink 0:9cf3d3dc621b 189 motorPWM = u;
ArvidKeemink 0:9cf3d3dc621b 190 motorDir = false;
ArvidKeemink 0:9cf3d3dc621b 191 } else {
ArvidKeemink 0:9cf3d3dc621b 192 motorPWM = -u;
ArvidKeemink 0:9cf3d3dc621b 193 motorDir = true;
ArvidKeemink 0:9cf3d3dc621b 194 }
ArvidKeemink 0:9cf3d3dc621b 195 }
ArvidKeemink 0:9cf3d3dc621b 196
ArvidKeemink 0:9cf3d3dc621b 197 void doNetwork()
ArvidKeemink 0:9cf3d3dc621b 198 {
ArvidKeemink 0:9cf3d3dc621b 199 //int size = eth.receive();
ArvidKeemink 0:9cf3d3dc621b 200 //if(size > 0)
ArvidKeemink 0:9cf3d3dc621b 201 //{
ArvidKeemink 0:9cf3d3dc621b 202 //eth.read(buf, size);
ArvidKeemink 0:9cf3d3dc621b 203
ArvidKeemink 0:9cf3d3dc621b 204 //}
ArvidKeemink 0:9cf3d3dc621b 205 }
ArvidKeemink 0:9cf3d3dc621b 206
ArvidKeemink 0:9cf3d3dc621b 207 void mapIn()
ArvidKeemink 0:9cf3d3dc621b 208 {
ArvidKeemink 0:9cf3d3dc621b 209 //Measure relevant signals
ArvidKeemink 0:9cf3d3dc621b 210 motorPos = (float)encoderPos / ENCODER_CPR * TWO_PI; //motor position in rad
ArvidKeemink 0:9cf3d3dc621b 211 encoderVel = (float)(encoderPos - encoderPos_prev) / LOOPDURATION; //encoder velocity in counts/s
ArvidKeemink 0:9cf3d3dc621b 212 encoderPos_prev = encoderPos; //encoder position in counts
ArvidKeemink 0:9cf3d3dc621b 213 motorVel = encoderVel / ENCODER_CPR * TWO_PI; //motor velocity in rad/s
ArvidKeemink 0:9cf3d3dc621b 214 motorVel = velocityFilter(motorVel); //filtered motor velocity in rad/s
ArvidKeemink 0:9cf3d3dc621b 215 motorPos_prev = motorPos; //Previous motor position in rad
ArvidKeemink 0:9cf3d3dc621b 216 force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force
ArvidKeemink 0:9cf3d3dc621b 217 doNetwork(); //Receive inputs over network (if available)
ArvidKeemink 0:9cf3d3dc621b 218 }
ArvidKeemink 0:9cf3d3dc621b 219
ArvidKeemink 0:9cf3d3dc621b 220 void mapOut()
ArvidKeemink 0:9cf3d3dc621b 221 {
ArvidKeemink 0:9cf3d3dc621b 222 //Apply control
ArvidKeemink 0:9cf3d3dc621b 223 setPWM(u);
ArvidKeemink 0:9cf3d3dc621b 224 //setPWM(0); //DEBUG ONLY
ArvidKeemink 0:9cf3d3dc621b 225 //Send network packets if required
ArvidKeemink 0:9cf3d3dc621b 226 }
ArvidKeemink 0:9cf3d3dc621b 227
ArvidKeemink 0:9cf3d3dc621b 228 //State functions--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 229
ArvidKeemink 0:9cf3d3dc621b 230 //During the wait/idle state
ArvidKeemink 0:9cf3d3dc621b 231 inline void doStateIdle()
ArvidKeemink 0:9cf3d3dc621b 232 {
ArvidKeemink 0:9cf3d3dc621b 233 //Control
ArvidKeemink 0:9cf3d3dc621b 234 u = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 235
ArvidKeemink 0:9cf3d3dc621b 236 //Handle LED
ArvidKeemink 0:9cf3d3dc621b 237 ledCnt++;
ArvidKeemink 0:9cf3d3dc621b 238 if (ledCnt > LEDPERIODS) {
ArvidKeemink 0:9cf3d3dc621b 239 ledState = !ledState;
ArvidKeemink 0:9cf3d3dc621b 240 if (ledState)
ArvidKeemink 0:9cf3d3dc621b 241 setLed(colorYellow);
ArvidKeemink 0:9cf3d3dc621b 242 else
ArvidKeemink 0:9cf3d3dc621b 243 setLed(colorBlack);
ArvidKeemink 0:9cf3d3dc621b 244 ledCnt = 0;
ArvidKeemink 0:9cf3d3dc621b 245 }
ArvidKeemink 0:9cf3d3dc621b 246
ArvidKeemink 0:9cf3d3dc621b 247 //State Guard:
ArvidKeemink 0:9cf3d3dc621b 248 if (t.read() > 3.0f) {
ArvidKeemink 0:9cf3d3dc621b 249 currentState = stateCalibration;
ArvidKeemink 0:9cf3d3dc621b 250 stateTime = t.read();
ArvidKeemink 0:9cf3d3dc621b 251 }
ArvidKeemink 0:9cf3d3dc621b 252 }
ArvidKeemink 0:9cf3d3dc621b 253
ArvidKeemink 0:9cf3d3dc621b 254 inline void doStateCalib()
ArvidKeemink 0:9cf3d3dc621b 255 {
ArvidKeemink 0:9cf3d3dc621b 256 //Control
ArvidKeemink 0:9cf3d3dc621b 257 //Send out calibration PWM
ArvidKeemink 0:9cf3d3dc621b 258 u = CALIBPWM;
ArvidKeemink 0:9cf3d3dc621b 259
ArvidKeemink 0:9cf3d3dc621b 260 //Handle LED
ArvidKeemink 0:9cf3d3dc621b 261 ledCnt++;
ArvidKeemink 0:9cf3d3dc621b 262 if (ledCnt > LEDPERIODS) {
ArvidKeemink 0:9cf3d3dc621b 263 ledState = !ledState;
ArvidKeemink 0:9cf3d3dc621b 264 if (ledState)
ArvidKeemink 0:9cf3d3dc621b 265 setLed(colorBlue);
ArvidKeemink 0:9cf3d3dc621b 266 else
ArvidKeemink 0:9cf3d3dc621b 267 setLed(colorBlack);
ArvidKeemink 0:9cf3d3dc621b 268 ledCnt = 0;
ArvidKeemink 0:9cf3d3dc621b 269 }
ArvidKeemink 0:9cf3d3dc621b 270
ArvidKeemink 0:9cf3d3dc621b 271 //State Guard
ArvidKeemink 0:9cf3d3dc621b 272 if ((abs(motorVel) < 0.001f) && (t.read()-stateTime > 1.0f)) {
ArvidKeemink 0:9cf3d3dc621b 273 currentState = stateHoming;
ArvidKeemink 0:9cf3d3dc621b 274 encoderPos = MAX_ENCODER_POS_LEFT; //When calibrated, set encoder pos to the known value
ArvidKeemink 0:9cf3d3dc621b 275 stateTime = t.read();
ArvidKeemink 0:9cf3d3dc621b 276 refPos = (float)MAX_ENCODER_POS_LEFT / ENCODER_CPR * TWO_PI; //Start at the left calibration point
ArvidKeemink 0:9cf3d3dc621b 277 }
ArvidKeemink 0:9cf3d3dc621b 278 }
ArvidKeemink 0:9cf3d3dc621b 279
ArvidKeemink 0:9cf3d3dc621b 280 inline void doStateHoming()
ArvidKeemink 0:9cf3d3dc621b 281 {
ArvidKeemink 0:9cf3d3dc621b 282 //Calculate a ramp to location 0 (center) with -2 rad/s
ArvidKeemink 0:9cf3d3dc621b 283 refVel = -2.00f;
ArvidKeemink 0:9cf3d3dc621b 284 if (refPos > 0.0f) {
ArvidKeemink 0:9cf3d3dc621b 285 refPos += refVel*LOOPDURATION;//refVel*LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 286 }
ArvidKeemink 0:9cf3d3dc621b 287 else {
ArvidKeemink 0:9cf3d3dc621b 288 refPos = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 289 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 290 }
ArvidKeemink 0:9cf3d3dc621b 291
ArvidKeemink 0:9cf3d3dc621b 292 //Control
ArvidKeemink 0:9cf3d3dc621b 293 posError = refPos - motorPos;
ArvidKeemink 0:9cf3d3dc621b 294 integratedError += posError*LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 295 velError = refVel - motorVel;
ArvidKeemink 0:9cf3d3dc621b 296 u = K_p * posError + K_d * velError + 0*K_i*integratedError;
ArvidKeemink 0:9cf3d3dc621b 297
ArvidKeemink 0:9cf3d3dc621b 298 //Handle LED
ArvidKeemink 0:9cf3d3dc621b 299 ledCnt++;
ArvidKeemink 0:9cf3d3dc621b 300 if (ledCnt > LEDPERIODS) {
ArvidKeemink 0:9cf3d3dc621b 301 ledState = !ledState;
ArvidKeemink 0:9cf3d3dc621b 302 if (ledState)
ArvidKeemink 0:9cf3d3dc621b 303 setLed(colorGreen);
ArvidKeemink 0:9cf3d3dc621b 304 else
ArvidKeemink 0:9cf3d3dc621b 305 setLed(colorBlack);
ArvidKeemink 0:9cf3d3dc621b 306 ledCnt = 0;
ArvidKeemink 0:9cf3d3dc621b 307 }
ArvidKeemink 0:9cf3d3dc621b 308
ArvidKeemink 0:9cf3d3dc621b 309 //State Guard
ArvidKeemink 0:9cf3d3dc621b 310 if ((abs(encoderPos) < 6) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
ArvidKeemink 0:9cf3d3dc621b 311 integratedError = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 312 setLed(colorGreen);
ArvidKeemink 0:9cf3d3dc621b 313 stateTime = t.read();
ArvidKeemink 0:9cf3d3dc621b 314 currentState = stateOPAdmittance;//stateOPImpedance;
ArvidKeemink 0:9cf3d3dc621b 315 /*if (doAdmittance) {
ArvidKeemink 0:9cf3d3dc621b 316 currentState = stateOPAdmittance;
ArvidKeemink 0:9cf3d3dc621b 317 } else {
ArvidKeemink 0:9cf3d3dc621b 318 currentState = stateOPImpedance;
ArvidKeemink 0:9cf3d3dc621b 319 }*/
ArvidKeemink 0:9cf3d3dc621b 320 }
ArvidKeemink 0:9cf3d3dc621b 321 }
ArvidKeemink 0:9cf3d3dc621b 322
ArvidKeemink 0:9cf3d3dc621b 323 inline void doStateOPAdmittance()
ArvidKeemink 0:9cf3d3dc621b 324 {
ArvidKeemink 0:9cf3d3dc621b 325 //Virtual Model
ArvidKeemink 0:9cf3d3dc621b 326 refAcc = force / virtualMass - virtualStiffness * (refPos - springPos) - virtualDamping * refVel;
ArvidKeemink 0:9cf3d3dc621b 327 refVel += refAcc * LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 328
ArvidKeemink 0:9cf3d3dc621b 329 if (refVel > MAXVEL)
ArvidKeemink 0:9cf3d3dc621b 330 {
ArvidKeemink 0:9cf3d3dc621b 331 refVel = MAXVEL;
ArvidKeemink 0:9cf3d3dc621b 332 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 333 } else if (refVel < -MAXVEL)
ArvidKeemink 0:9cf3d3dc621b 334 {
ArvidKeemink 0:9cf3d3dc621b 335 refVel = -MAXVEL;
ArvidKeemink 0:9cf3d3dc621b 336 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 337 }
ArvidKeemink 0:9cf3d3dc621b 338
ArvidKeemink 0:9cf3d3dc621b 339 refPos += refVel * LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 340 if (refPos > WORKSPACEBOUND)
ArvidKeemink 0:9cf3d3dc621b 341 {
ArvidKeemink 0:9cf3d3dc621b 342 refPos = WORKSPACEBOUND;
ArvidKeemink 0:9cf3d3dc621b 343 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 344 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 345 } else if (refPos < -WORKSPACEBOUND)
ArvidKeemink 0:9cf3d3dc621b 346 {
ArvidKeemink 0:9cf3d3dc621b 347 refPos = -WORKSPACEBOUND;
ArvidKeemink 0:9cf3d3dc621b 348 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 349 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 350 }
ArvidKeemink 0:9cf3d3dc621b 351
ArvidKeemink 0:9cf3d3dc621b 352 //Controller
ArvidKeemink 0:9cf3d3dc621b 353 posError = refPos - motorPos;
ArvidKeemink 0:9cf3d3dc621b 354 velError = refVel - motorVel;
ArvidKeemink 0:9cf3d3dc621b 355 integratedError += posError*LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 356 u = refAcc*I_ff + K_p * posError + K_d * velError + 0*K_i * integratedError;
ArvidKeemink 0:9cf3d3dc621b 357
ArvidKeemink 0:9cf3d3dc621b 358 //StateGuard
ArvidKeemink 0:9cf3d3dc621b 359 //if...
ArvidKeemink 0:9cf3d3dc621b 360 }
ArvidKeemink 0:9cf3d3dc621b 361
ArvidKeemink 0:9cf3d3dc621b 362 inline void doStateOPImpedance()
ArvidKeemink 0:9cf3d3dc621b 363 {
ArvidKeemink 0:9cf3d3dc621b 364 //Controller is the virtual model in OPEN LOOP impedance
ArvidKeemink 0:9cf3d3dc621b 365 u = - virtualStiffness * (motorPos - springPos) - virtualDamping * motorVel;
ArvidKeemink 0:9cf3d3dc621b 366 //StateGuard
ArvidKeemink 0:9cf3d3dc621b 367 //if...
ArvidKeemink 0:9cf3d3dc621b 368 }
ArvidKeemink 0:9cf3d3dc621b 369
ArvidKeemink 0:9cf3d3dc621b 370 inline void doStateFailed()
ArvidKeemink 0:9cf3d3dc621b 371 {
ArvidKeemink 0:9cf3d3dc621b 372 //Control
ArvidKeemink 0:9cf3d3dc621b 373 u = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 374
ArvidKeemink 0:9cf3d3dc621b 375 //Handle LED
ArvidKeemink 0:9cf3d3dc621b 376 ledCnt++;
ArvidKeemink 0:9cf3d3dc621b 377 if (ledCnt > LEDPERIODS) {
ArvidKeemink 0:9cf3d3dc621b 378 ledState = !ledState;
ArvidKeemink 0:9cf3d3dc621b 379 if (ledState)
ArvidKeemink 0:9cf3d3dc621b 380 setLed(colorRed);
ArvidKeemink 0:9cf3d3dc621b 381 else
ArvidKeemink 0:9cf3d3dc621b 382 setLed(colorBlack);
ArvidKeemink 0:9cf3d3dc621b 383 ledCnt = 0;
ArvidKeemink 0:9cf3d3dc621b 384 }
ArvidKeemink 0:9cf3d3dc621b 385
ArvidKeemink 0:9cf3d3dc621b 386 //No state guard.
ArvidKeemink 0:9cf3d3dc621b 387 }
ArvidKeemink 0:9cf3d3dc621b 388
ArvidKeemink 0:9cf3d3dc621b 389 //Loop Function--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 390 void loopFunction()
ArvidKeemink 0:9cf3d3dc621b 391 {
ArvidKeemink 0:9cf3d3dc621b 392 //This code is run every loop
ArvidKeemink 0:9cf3d3dc621b 393 loopCounter++;
ArvidKeemink 0:9cf3d3dc621b 394
ArvidKeemink 0:9cf3d3dc621b 395 //Measure relevant inputs
ArvidKeemink 0:9cf3d3dc621b 396 mapIn();
ArvidKeemink 0:9cf3d3dc621b 397
ArvidKeemink 0:9cf3d3dc621b 398 //Set Defaults
ArvidKeemink 0:9cf3d3dc621b 399 u = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 400
ArvidKeemink 0:9cf3d3dc621b 401 //Do State Machine
ArvidKeemink 0:9cf3d3dc621b 402 switch(currentState) {
ArvidKeemink 0:9cf3d3dc621b 403 case stateIdle:
ArvidKeemink 0:9cf3d3dc621b 404 doStateIdle();
ArvidKeemink 0:9cf3d3dc621b 405 break;
ArvidKeemink 0:9cf3d3dc621b 406 case stateCalibration:
ArvidKeemink 0:9cf3d3dc621b 407 doStateCalib();
ArvidKeemink 0:9cf3d3dc621b 408 break;
ArvidKeemink 0:9cf3d3dc621b 409 case stateHoming:
ArvidKeemink 0:9cf3d3dc621b 410 doStateHoming();
ArvidKeemink 0:9cf3d3dc621b 411 break;
ArvidKeemink 0:9cf3d3dc621b 412 case stateOPImpedance:
ArvidKeemink 0:9cf3d3dc621b 413 doStateOPImpedance();
ArvidKeemink 0:9cf3d3dc621b 414 break;
ArvidKeemink 0:9cf3d3dc621b 415 case stateOPAdmittance:
ArvidKeemink 0:9cf3d3dc621b 416 doStateOPAdmittance();
ArvidKeemink 0:9cf3d3dc621b 417 break;
ArvidKeemink 0:9cf3d3dc621b 418 case stateFailed: //fall through
ArvidKeemink 0:9cf3d3dc621b 419 doStateFailed();
ArvidKeemink 0:9cf3d3dc621b 420 break;
ArvidKeemink 0:9cf3d3dc621b 421 }
ArvidKeemink 0:9cf3d3dc621b 422
ArvidKeemink 0:9cf3d3dc621b 423 mapOut(); //Set all relevant output pins
ArvidKeemink 0:9cf3d3dc621b 424 }
ArvidKeemink 0:9cf3d3dc621b 425
ArvidKeemink 0:9cf3d3dc621b 426 //Main--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 427 int main()
ArvidKeemink 0:9cf3d3dc621b 428 {
ArvidKeemink 0:9cf3d3dc621b 429 setLed(colorBlack); //Set LED to black
ArvidKeemink 0:9cf3d3dc621b 430 pc.baud(115200); //set Serial comm
ArvidKeemink 0:9cf3d3dc621b 431 t.start();
ArvidKeemink 0:9cf3d3dc621b 432
ArvidKeemink 0:9cf3d3dc621b 433 //Set all interrupt requests to 2nd place priority
ArvidKeemink 0:9cf3d3dc621b 434 for(int n = 0; n < 86; n++) {
ArvidKeemink 0:9cf3d3dc621b 435 NVIC_SetPriority((IRQn)n,1);
ArvidKeemink 0:9cf3d3dc621b 436 }
ArvidKeemink 0:9cf3d3dc621b 437 //Set out motor encoder interrupt to 1st place priority
ArvidKeemink 0:9cf3d3dc621b 438 NVIC_SetPriority(PORTB_IRQn,0);
ArvidKeemink 0:9cf3d3dc621b 439
ArvidKeemink 0:9cf3d3dc621b 440
ArvidKeemink 0:9cf3d3dc621b 441 EncoderA.rise(&encoderFunctionA); //Attach interrupts only to rising edge encoder channel A
ArvidKeemink 0:9cf3d3dc621b 442 motorPWM.period_us(PWMCYCLETIMEUS); //50 us = 20 kHz
ArvidKeemink 0:9cf3d3dc621b 443 mainLoop.attach_us(&loopFunction,LOOPDURATIONUS); //200 us = 5 kHz
ArvidKeemink 0:9cf3d3dc621b 444
ArvidKeemink 0:9cf3d3dc621b 445 //Infinite while loop
ArvidKeemink 0:9cf3d3dc621b 446 while (true) {
ArvidKeemink 0:9cf3d3dc621b 447 //Print relevant stuff to terminal here, do not print in the in the loopFunction ticker!!
ArvidKeemink 0:9cf3d3dc621b 448 if (currentState != statePrev) { //When state changes
ArvidKeemink 0:9cf3d3dc621b 449 pc.printf(">>>>>State Changed to State: %d\r\n",currentState);
ArvidKeemink 0:9cf3d3dc621b 450 statePrev = currentState; //Update the prev state var
ArvidKeemink 0:9cf3d3dc621b 451 }
ArvidKeemink 0:9cf3d3dc621b 452 wait(0.01);
ArvidKeemink 0:9cf3d3dc621b 453 float millisecondsPassed = (float)loopCounter*(((float)LOOPDURATIONUS) / 1000.0f);
ArvidKeemink 0:9cf3d3dc621b 454 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);
ArvidKeemink 0:9cf3d3dc621b 455 }
ArvidKeemink 0:9cf3d3dc621b 456 }
ArvidKeemink 0:9cf3d3dc621b 457 //EOF--------------------------------------------------------------------------------------------------------