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@1:08d285e33eea, 2018-06-11 (annotated)

Committer:
biek
Date:
Mon Jun 11 07:47:13 2018 +0000
Revision:
1:08d285e33eea
Parent:
0:9cf3d3dc621b
Child:
2:306998d368fc
Partially implemented QEI: still need to update calibration.

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