Arvid Keemink / Mbed 2 deprecated BMHaptics1

Example Software

Dependencies:   FastPWM MODSERIAL mbed QEI

main.cpp@7:c662c825d755, 2018-06-11 (annotated)

Committer:
biek
Date:
Mon Jun 11 09:53:29 2018 +0000
Revision:
7:c662c825d755
Parent:
6:ae4a619be005 
Force sensor working, admittance control tested, bug in encoder reset fixed (on state change).

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
biek 6:ae4a619be005 14 #define CALIBPWM -0.1f //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
biek 7:c662c825d755 19 #define FORCESENSORGAIN 10.0f //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;
biek 7:c662c825d755 82 const float virtualStiffness = 0.05f;
biek 7:c662c825d755 83 const float virtualDamping = 0.005f;
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;
biek 6:ae4a619be005 182 motorDir = true;
ArvidKeemink 0:9cf3d3dc621b 183 } else {
ArvidKeemink 0:9cf3d3dc621b 184 motorPWM = -u;
biek 6:ae4a619be005 185 motorDir = false;
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
ArvidKeemink 5:f7e4ecadffd2 202 encoderPos = enc.getPulses();
ArvidKeemink 5:f7e4ecadffd2 203 motorPos = (float)encoderPos / 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
biek 7:c662c825d755 209 force = FORCESENSORGAIN*(measuredForce - 0.505f); //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;
biek 2:306998d368fc 267 enc.reset(); // set pulses to zero
biek 2:306998d368fc 268
biek 2:306998d368fc 269 // NEED TO UPDATE CALIBRATION HERE.
biek 2:306998d368fc 270 //encoderPos = -MAX_ENCODER_POS_LEFT; //When calibrated, set encoder pos to the known value
ArvidKeemink 0:9cf3d3dc621b 271 stateTime = t.read();
ArvidKeemink 5:f7e4ecadffd2 272 refPos = 0;//(float)MAX_ENCODER_POS_LEFT / ENCODER_CPR * TWO_PI; //Start at the left calibration point
ArvidKeemink 0:9cf3d3dc621b 273 }
ArvidKeemink 0:9cf3d3dc621b 274 }
ArvidKeemink 0:9cf3d3dc621b 275
ArvidKeemink 0:9cf3d3dc621b 276 inline void doStateHoming()
ArvidKeemink 0:9cf3d3dc621b 277 {
ArvidKeemink 0:9cf3d3dc621b 278 //Calculate a ramp to location 0 (center) with -2 rad/s
biek 6:ae4a619be005 279 refVel = 2.00f;
biek 6:ae4a619be005 280 if (refPos < 9.296f) {
biek 6:ae4a619be005 281 refPos += refVel*LOOPDURATION;//refVel*LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 282 }
ArvidKeemink 0:9cf3d3dc621b 283 else {
biek 6:ae4a619be005 284 refPos = 9.296f;
ArvidKeemink 0:9cf3d3dc621b 285 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 286 }
ArvidKeemink 0:9cf3d3dc621b 287
ArvidKeemink 0:9cf3d3dc621b 288 //Control
ArvidKeemink 0:9cf3d3dc621b 289 posError = refPos - motorPos;
ArvidKeemink 0:9cf3d3dc621b 290 integratedError += posError*LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 291 velError = refVel - motorVel;
ArvidKeemink 0:9cf3d3dc621b 292 u = K_p * posError + K_d * velError + 0*K_i*integratedError;
ArvidKeemink 0:9cf3d3dc621b 293
ArvidKeemink 0:9cf3d3dc621b 294 //Handle LED
ArvidKeemink 0:9cf3d3dc621b 295 ledCnt++;
ArvidKeemink 0:9cf3d3dc621b 296 if (ledCnt > LEDPERIODS) {
ArvidKeemink 0:9cf3d3dc621b 297 ledState = !ledState;
ArvidKeemink 0:9cf3d3dc621b 298 if (ledState)
ArvidKeemink 0:9cf3d3dc621b 299 setLed(colorGreen);
ArvidKeemink 0:9cf3d3dc621b 300 else
ArvidKeemink 0:9cf3d3dc621b 301 setLed(colorBlack);
ArvidKeemink 0:9cf3d3dc621b 302 ledCnt = 0;
ArvidKeemink 0:9cf3d3dc621b 303 }
ArvidKeemink 0:9cf3d3dc621b 304
ArvidKeemink 0:9cf3d3dc621b 305 //State Guard
biek 6:ae4a619be005 306 if ((abs(posError) < 0.01f) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
biek 7:c662c825d755 307 refPos = 0.0f;
biek 7:c662c825d755 308 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 309 integratedError = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 310 setLed(colorGreen);
ArvidKeemink 0:9cf3d3dc621b 311 stateTime = t.read();
biek 7:c662c825d755 312 currentState = stateOPAdmittance;//stateOPImpedance;
biek 7:c662c825d755 313 //currentState = stateFailed;
ArvidKeemink 5:f7e4ecadffd2 314
ArvidKeemink 5:f7e4ecadffd2 315 //Reset encoder back to 0 in center:
ArvidKeemink 5:f7e4ecadffd2 316 enc.reset();
biek 7:c662c825d755 317 encoderPos_prev = 0;
ArvidKeemink 5:f7e4ecadffd2 318
ArvidKeemink 0:9cf3d3dc621b 319 /*if (doAdmittance) {
ArvidKeemink 0:9cf3d3dc621b 320 currentState = stateOPAdmittance;
ArvidKeemink 0:9cf3d3dc621b 321 } else {
ArvidKeemink 0:9cf3d3dc621b 322 currentState = stateOPImpedance;
ArvidKeemink 0:9cf3d3dc621b 323 }*/
ArvidKeemink 0:9cf3d3dc621b 324 }
ArvidKeemink 0:9cf3d3dc621b 325 }
ArvidKeemink 0:9cf3d3dc621b 326
ArvidKeemink 0:9cf3d3dc621b 327 inline void doStateOPAdmittance()
ArvidKeemink 0:9cf3d3dc621b 328 {
ArvidKeemink 0:9cf3d3dc621b 329 //Virtual Model
biek 7:c662c825d755 330 refAcc = (force - virtualStiffness * (refPos - springPos) - virtualDamping * refVel) / virtualMass;
ArvidKeemink 0:9cf3d3dc621b 331 refVel += refAcc * LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 332
ArvidKeemink 0:9cf3d3dc621b 333 if (refVel > MAXVEL)
ArvidKeemink 0:9cf3d3dc621b 334 {
ArvidKeemink 0:9cf3d3dc621b 335 refVel = MAXVEL;
ArvidKeemink 0:9cf3d3dc621b 336 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 337 } else if (refVel < -MAXVEL)
ArvidKeemink 0:9cf3d3dc621b 338 {
ArvidKeemink 0:9cf3d3dc621b 339 refVel = -MAXVEL;
ArvidKeemink 0:9cf3d3dc621b 340 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 341 }
ArvidKeemink 0:9cf3d3dc621b 342
ArvidKeemink 0:9cf3d3dc621b 343 refPos += refVel * LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 344 if (refPos > WORKSPACEBOUND)
ArvidKeemink 0:9cf3d3dc621b 345 {
ArvidKeemink 0:9cf3d3dc621b 346 refPos = WORKSPACEBOUND;
ArvidKeemink 0:9cf3d3dc621b 347 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 348 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 349 } else if (refPos < -WORKSPACEBOUND)
ArvidKeemink 0:9cf3d3dc621b 350 {
ArvidKeemink 0:9cf3d3dc621b 351 refPos = -WORKSPACEBOUND;
ArvidKeemink 0:9cf3d3dc621b 352 refVel = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 353 refAcc = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 354 }
ArvidKeemink 0:9cf3d3dc621b 355
ArvidKeemink 0:9cf3d3dc621b 356 //Controller
ArvidKeemink 0:9cf3d3dc621b 357 posError = refPos - motorPos;
ArvidKeemink 0:9cf3d3dc621b 358 velError = refVel - motorVel;
ArvidKeemink 0:9cf3d3dc621b 359 integratedError += posError*LOOPDURATION;
ArvidKeemink 0:9cf3d3dc621b 360 u = refAcc*I_ff + K_p * posError + K_d * velError + 0*K_i * integratedError;
ArvidKeemink 0:9cf3d3dc621b 361
ArvidKeemink 0:9cf3d3dc621b 362 //StateGuard
ArvidKeemink 0:9cf3d3dc621b 363 //if...
ArvidKeemink 0:9cf3d3dc621b 364 }
ArvidKeemink 0:9cf3d3dc621b 365
ArvidKeemink 0:9cf3d3dc621b 366 inline void doStateOPImpedance()
ArvidKeemink 0:9cf3d3dc621b 367 {
ArvidKeemink 0:9cf3d3dc621b 368 //Controller is the virtual model in OPEN LOOP impedance
ArvidKeemink 0:9cf3d3dc621b 369 u = - virtualStiffness * (motorPos - springPos) - virtualDamping * motorVel;
ArvidKeemink 0:9cf3d3dc621b 370 //StateGuard
ArvidKeemink 0:9cf3d3dc621b 371 //if...
ArvidKeemink 0:9cf3d3dc621b 372 }
ArvidKeemink 0:9cf3d3dc621b 373
ArvidKeemink 0:9cf3d3dc621b 374 inline void doStateFailed()
ArvidKeemink 0:9cf3d3dc621b 375 {
ArvidKeemink 0:9cf3d3dc621b 376 //Control
ArvidKeemink 0:9cf3d3dc621b 377 u = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 378
ArvidKeemink 0:9cf3d3dc621b 379 //Handle LED
ArvidKeemink 0:9cf3d3dc621b 380 ledCnt++;
ArvidKeemink 0:9cf3d3dc621b 381 if (ledCnt > LEDPERIODS) {
ArvidKeemink 0:9cf3d3dc621b 382 ledState = !ledState;
ArvidKeemink 0:9cf3d3dc621b 383 if (ledState)
ArvidKeemink 0:9cf3d3dc621b 384 setLed(colorRed);
ArvidKeemink 0:9cf3d3dc621b 385 else
ArvidKeemink 0:9cf3d3dc621b 386 setLed(colorBlack);
ArvidKeemink 0:9cf3d3dc621b 387 ledCnt = 0;
ArvidKeemink 0:9cf3d3dc621b 388 }
ArvidKeemink 0:9cf3d3dc621b 389
ArvidKeemink 0:9cf3d3dc621b 390 //No state guard.
ArvidKeemink 0:9cf3d3dc621b 391 }
ArvidKeemink 0:9cf3d3dc621b 392
ArvidKeemink 0:9cf3d3dc621b 393 //Loop Function--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 394 void loopFunction()
ArvidKeemink 0:9cf3d3dc621b 395 {
ArvidKeemink 0:9cf3d3dc621b 396 //This code is run every loop
ArvidKeemink 0:9cf3d3dc621b 397 loopCounter++;
ArvidKeemink 0:9cf3d3dc621b 398
ArvidKeemink 0:9cf3d3dc621b 399 //Measure relevant inputs
ArvidKeemink 0:9cf3d3dc621b 400 mapIn();
ArvidKeemink 0:9cf3d3dc621b 401
ArvidKeemink 0:9cf3d3dc621b 402 //Set Defaults
ArvidKeemink 0:9cf3d3dc621b 403 u = 0.0f;
ArvidKeemink 0:9cf3d3dc621b 404
ArvidKeemink 0:9cf3d3dc621b 405 //Do State Machine
ArvidKeemink 0:9cf3d3dc621b 406 switch(currentState) {
ArvidKeemink 0:9cf3d3dc621b 407 case stateIdle:
ArvidKeemink 0:9cf3d3dc621b 408 doStateIdle();
ArvidKeemink 0:9cf3d3dc621b 409 break;
ArvidKeemink 0:9cf3d3dc621b 410 case stateCalibration:
ArvidKeemink 0:9cf3d3dc621b 411 doStateCalib();
ArvidKeemink 0:9cf3d3dc621b 412 break;
ArvidKeemink 0:9cf3d3dc621b 413 case stateHoming:
ArvidKeemink 0:9cf3d3dc621b 414 doStateHoming();
ArvidKeemink 0:9cf3d3dc621b 415 break;
ArvidKeemink 0:9cf3d3dc621b 416 case stateOPImpedance:
ArvidKeemink 0:9cf3d3dc621b 417 doStateOPImpedance();
ArvidKeemink 0:9cf3d3dc621b 418 break;
ArvidKeemink 0:9cf3d3dc621b 419 case stateOPAdmittance:
ArvidKeemink 0:9cf3d3dc621b 420 doStateOPAdmittance();
ArvidKeemink 0:9cf3d3dc621b 421 break;
ArvidKeemink 0:9cf3d3dc621b 422 case stateFailed: //fall through
ArvidKeemink 0:9cf3d3dc621b 423 doStateFailed();
ArvidKeemink 0:9cf3d3dc621b 424 break;
ArvidKeemink 0:9cf3d3dc621b 425 }
ArvidKeemink 0:9cf3d3dc621b 426
ArvidKeemink 0:9cf3d3dc621b 427 mapOut(); //Set all relevant output pins
ArvidKeemink 0:9cf3d3dc621b 428 }
ArvidKeemink 0:9cf3d3dc621b 429
ArvidKeemink 0:9cf3d3dc621b 430 //Main--------------------------------------------------------------------------------------------------------
ArvidKeemink 0:9cf3d3dc621b 431 int main()
ArvidKeemink 0:9cf3d3dc621b 432 {
ArvidKeemink 0:9cf3d3dc621b 433 setLed(colorBlack); //Set LED to black
ArvidKeemink 0:9cf3d3dc621b 434 pc.baud(115200); //set Serial comm
ArvidKeemink 0:9cf3d3dc621b 435 t.start();
ArvidKeemink 0:9cf3d3dc621b 436
ArvidKeemink 0:9cf3d3dc621b 437 //Set all interrupt requests to 2nd place priority
ArvidKeemink 0:9cf3d3dc621b 438 for(int n = 0; n < 86; n++) {
ArvidKeemink 0:9cf3d3dc621b 439 NVIC_SetPriority((IRQn)n,1);
ArvidKeemink 0:9cf3d3dc621b 440 }
ArvidKeemink 0:9cf3d3dc621b 441 //Set out motor encoder interrupt to 1st place priority
biek 3:5cb5f0f22c5a 442 NVIC_SetPriority(PORTC_IRQn,0);
ArvidKeemink 0:9cf3d3dc621b 443
ArvidKeemink 0:9cf3d3dc621b 444 motorPWM.period_us(PWMCYCLETIMEUS); //50 us = 20 kHz
ArvidKeemink 0:9cf3d3dc621b 445 mainLoop.attach_us(&loopFunction,LOOPDURATIONUS); //200 us = 5 kHz
ArvidKeemink 0:9cf3d3dc621b 446
ArvidKeemink 0:9cf3d3dc621b 447 //Infinite while loop
ArvidKeemink 0:9cf3d3dc621b 448 while (true) {
ArvidKeemink 0:9cf3d3dc621b 449 //Print relevant stuff to terminal here, do not print in the in the loopFunction ticker!!
ArvidKeemink 0:9cf3d3dc621b 450 if (currentState != statePrev) { //When state changes
ArvidKeemink 0:9cf3d3dc621b 451 pc.printf(">>>>>State Changed to State: %d\r\n",currentState);
ArvidKeemink 0:9cf3d3dc621b 452 statePrev = currentState; //Update the prev state var
ArvidKeemink 0:9cf3d3dc621b 453 }
ArvidKeemink 0:9cf3d3dc621b 454 wait(0.01);
ArvidKeemink 0:9cf3d3dc621b 455 float millisecondsPassed = (float)loopCounter*(((float)LOOPDURATIONUS) / 1000.0f);
biek 7:c662c825d755 456 pc.printf("t:%f;State:%d;EncPos:%d;MotPos:%f;MotVel:%f;Force:%f;RefPos:%f\r\n",millisecondsPassed,currentState,encoderPos,motorPos,motorVel,force,refPos);//,posError,u); ;PosErr:%f;MotPWM:%f
ArvidKeemink 0:9cf3d3dc621b 457 }
ArvidKeemink 0:9cf3d3dc621b 458 }
ArvidKeemink 0:9cf3d3dc621b 459 //EOF--------------------------------------------------------------------------------------------------------