Artem Solomatin / Mbed 2 deprecated InvertedPendulum

Inverted pendulum lab firmware

Dependencies:   mbed

main.cpp@0:06a18c897777, 2020-05-28 (annotated)

Committer:
teamat
Date:
Thu May 28 14:33:13 2020 +0000
Revision:
0:06a18c897777
Finished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teamat 0:06a18c897777 1 #include "mbed.h"
teamat 0:06a18c897777 2
teamat 0:06a18c897777 3 #define M_PI 3.14159265358979323846f
teamat 0:06a18c897777 4
teamat 0:06a18c897777 5 #define STATE_ERROR 0
teamat 0:06a18c897777 6 #define STATE_INIT 1
teamat 0:06a18c897777 7 #define STATE_GOTO_START 2
teamat 0:06a18c897777 8 #define STATE_GOTO_END_COUNTING 3
teamat 0:06a18c897777 9 #define STATE_GOTO_MIDDLE 4
teamat 0:06a18c897777 10 #define STATE_WAITING 5
teamat 0:06a18c897777 11 #define STATE_GOTO_SWING 6
teamat 0:06a18c897777 12 #define STATE_START_SWING 7
teamat 0:06a18c897777 13 #define STATE_SWING_RIGHT 8
teamat 0:06a18c897777 14 #define STATE_SWING_LEFT 9
teamat 0:06a18c897777 15 #define STATE_EMPTY_SWING 10
teamat 0:06a18c897777 16
teamat 0:06a18c897777 17 #define DIR_LEFT 0
teamat 0:06a18c897777 18 #define DIR_RIGHT 1
teamat 0:06a18c897777 19
teamat 0:06a18c897777 20 #define timer_period_us 10
teamat 0:06a18c897777 21
teamat 0:06a18c897777 22
teamat 0:06a18c897777 23 DigitalOut RCout(D10);
teamat 0:06a18c897777 24 DigitalOut dir(D9);
teamat 0:06a18c897777 25 InterruptIn leftSwitch(D2);
teamat 0:06a18c897777 26 InterruptIn rightSwitch(D3);
teamat 0:06a18c897777 27 Ticker tick;
teamat 0:06a18c897777 28 Ticker speedTicker;
teamat 0:06a18c897777 29 Ticker swingTicker;
teamat 0:06a18c897777 30 RawSerial rpc(D1,D0,9600);
teamat 0:06a18c897777 31 DigitalOut led(D4);
teamat 0:06a18c897777 32
teamat 0:06a18c897777 33 int period_us = 1;
teamat 0:06a18c897777 34 int timerCounter = 0;
teamat 0:06a18c897777 35
teamat 0:06a18c897777 36 long pos=0;
teamat 0:06a18c897777 37
teamat 0:06a18c897777 38 long railLength = 0;
teamat 0:06a18c897777 39
teamat 0:06a18c897777 40 uint8_t state=STATE_INIT;
teamat 0:06a18c897777 41 uint8_t data[2] = {0, 0};
teamat 0:06a18c897777 42 uint8_t cmdIndex = 0;
teamat 0:06a18c897777 43
teamat 0:06a18c897777 44 Timer t;
teamat 0:06a18c897777 45 SPI spi(D11,D12,D13);// mosi, miso, sclk
teamat 0:06a18c897777 46 DigitalOut cs(D5);
teamat 0:06a18c897777 47
teamat 0:06a18c897777 48 float angularPosNew = 0;
teamat 0:06a18c897777 49 float angularPosOld = 0;
teamat 0:06a18c897777 50
teamat 0:06a18c897777 51 float timeOld = 0.0f;
teamat 0:06a18c897777 52 float timeStart = 0.0f;
teamat 0:06a18c897777 53
teamat 0:06a18c897777 54 float dx = 0;
teamat 0:06a18c897777 55 float xPosNew = 0;
teamat 0:06a18c897777 56 float xPosOld = 0;
teamat 0:06a18c897777 57 float speed = -1;
teamat 0:06a18c897777 58 float posOffset = 0.0f;
teamat 0:06a18c897777 59
teamat 0:06a18c897777 60 double dAngle = 0.0f;
teamat 0:06a18c897777 61 double anSpd = 0.0f;
teamat 0:06a18c897777 62
teamat 0:06a18c897777 63 float timeOldPos = 0.0f;
teamat 0:06a18c897777 64 float timeStartPos = 0.0f;
teamat 0:06a18c897777 65
teamat 0:06a18c897777 66 float angle=0.0f;
teamat 0:06a18c897777 67 float angleOffset = 0;
teamat 0:06a18c897777 68
teamat 0:06a18c897777 69 float control = 0.0f;
teamat 0:06a18c897777 70
teamat 0:06a18c897777 71 double twoPI = 6.28;
teamat 0:06a18c897777 72
teamat 0:06a18c897777 73 bool newData = false;
teamat 0:06a18c897777 74 bool canSend = false;
teamat 0:06a18c897777 75
teamat 0:06a18c897777 76
teamat 0:06a18c897777 77 typedef union {
teamat 0:06a18c897777 78 float number[6];
teamat 0:06a18c897777 79 uint8_t numberCh[24];
teamat 0:06a18c897777 80 } my_union;
teamat 0:06a18c897777 81
teamat 0:06a18c897777 82 my_union myUnion;
teamat 0:06a18c897777 83
teamat 0:06a18c897777 84 bool isPendulumSwinging() {
teamat 0:06a18c897777 85 return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
teamat 0:06a18c897777 86 }
teamat 0:06a18c897777 87
teamat 0:06a18c897777 88 float getPosMM() {
teamat 0:06a18c897777 89 return pos / 3500.0f;
teamat 0:06a18c897777 90 }
teamat 0:06a18c897777 91
teamat 0:06a18c897777 92 uint16_t getPendulumPos(){
teamat 0:06a18c897777 93 cs=0;
teamat 0:06a18c897777 94 wait_ms(1);
teamat 0:06a18c897777 95 uint16_t d=spi.write((short)0x00);
teamat 0:06a18c897777 96 d=d<<1;
teamat 0:06a18c897777 97 d=d>>6;
teamat 0:06a18c897777 98 cs=1;
teamat 0:06a18c897777 99 wait_ms(1);
teamat 0:06a18c897777 100 return (uint16_t)d;
teamat 0:06a18c897777 101 }
teamat 0:06a18c897777 102
teamat 0:06a18c897777 103
teamat 0:06a18c897777 104 float getPendulumAngle(){
teamat 0:06a18c897777 105 angle = getPendulumPos();
teamat 0:06a18c897777 106 angle = angle * 6.28f / 1024.0f;
teamat 0:06a18c897777 107 angle += angleOffset;
teamat 0:06a18c897777 108 if (angle > 3.14) {
teamat 0:06a18c897777 109 angle = angle - twoPI;
teamat 0:06a18c897777 110 }
teamat 0:06a18c897777 111 return angle;
teamat 0:06a18c897777 112 }
teamat 0:06a18c897777 113
teamat 0:06a18c897777 114
teamat 0:06a18c897777 115 /*
teamat 0:06a18c897777 116 ANGULAR SPEED CALC
teamat 0:06a18c897777 117 */
teamat 0:06a18c897777 118
teamat 0:06a18c897777 119 void getDeltaAng(){
teamat 0:06a18c897777 120
teamat 0:06a18c897777 121 angularPosNew = getPendulumAngle();
teamat 0:06a18c897777 122
teamat 0:06a18c897777 123 dAngle = fmod((angularPosNew - angularPosOld + 3.14), twoPI) - 3.14;
teamat 0:06a18c897777 124 if (dAngle < -3.14) {
teamat 0:06a18c897777 125 dAngle += twoPI;
teamat 0:06a18c897777 126 }
teamat 0:06a18c897777 127
teamat 0:06a18c897777 128 angularPosOld = angularPosNew;
teamat 0:06a18c897777 129
teamat 0:06a18c897777 130 }
teamat 0:06a18c897777 131
teamat 0:06a18c897777 132 void getAngularSpeed(){
teamat 0:06a18c897777 133 float deltaTime;
teamat 0:06a18c897777 134 timeStart = float(t.read());
teamat 0:06a18c897777 135 deltaTime = (timeStart - timeOld);
teamat 0:06a18c897777 136 getDeltaAng();
teamat 0:06a18c897777 137 anSpd = dAngle / deltaTime;
teamat 0:06a18c897777 138 timeOld=timeStart;
teamat 0:06a18c897777 139 }
teamat 0:06a18c897777 140
teamat 0:06a18c897777 141 /*
teamat 0:06a18c897777 142 SPEED CALC
teamat 0:06a18c897777 143 */
teamat 0:06a18c897777 144
teamat 0:06a18c897777 145
teamat 0:06a18c897777 146 void calcSpeed() {
teamat 0:06a18c897777 147 xPosNew = (getPosMM() - posOffset) / 1000;
teamat 0:06a18c897777 148 speed = xPosNew - xPosOld;
teamat 0:06a18c897777 149 if (dir == DIR_LEFT) {
teamat 0:06a18c897777 150 speed = -speed;
teamat 0:06a18c897777 151 }
teamat 0:06a18c897777 152 xPosOld = xPosNew;
teamat 0:06a18c897777 153 }
teamat 0:06a18c897777 154
teamat 0:06a18c897777 155 void calcControl() {
teamat 0:06a18c897777 156 control = (dir.read() * 2 - 1) *(period_us * timer_period_us);
teamat 0:06a18c897777 157 }
teamat 0:06a18c897777 158
teamat 0:06a18c897777 159
teamat 0:06a18c897777 160
teamat 0:06a18c897777 161 void stepperFlip() {
teamat 0:06a18c897777 162 if (timerCounter * timer_period_us > period_us) {
teamat 0:06a18c897777 163 timerCounter = 0;
teamat 0:06a18c897777 164 if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
teamat 0:06a18c897777 165 RCout = !RCout;
teamat 0:06a18c897777 166 pos += (dir.read() * 2 - 1);
teamat 0:06a18c897777 167 }
teamat 0:06a18c897777 168 if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
teamat 0:06a18c897777 169 posOffset = 40;
teamat 0:06a18c897777 170 state = STATE_WAITING;
teamat 0:06a18c897777 171 }
teamat 0:06a18c897777 172 if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
teamat 0:06a18c897777 173 pos += (dir.read() * 2 - 1);
teamat 0:06a18c897777 174 RCout = !RCout;
teamat 0:06a18c897777 175 }
teamat 0:06a18c897777 176 } else {
teamat 0:06a18c897777 177 timerCounter += 1;
teamat 0:06a18c897777 178 }
teamat 0:06a18c897777 179 }
teamat 0:06a18c897777 180
teamat 0:06a18c897777 181 void leftEnd() {
teamat 0:06a18c897777 182 dir = DIR_RIGHT;
teamat 0:06a18c897777 183 if (state == STATE_GOTO_START) {
teamat 0:06a18c897777 184 state = STATE_GOTO_END_COUNTING;
teamat 0:06a18c897777 185 pos = 0;
teamat 0:06a18c897777 186 }
teamat 0:06a18c897777 187 else if (isPendulumSwinging()) {
teamat 0:06a18c897777 188 state = STATE_ERROR;
teamat 0:06a18c897777 189 }
teamat 0:06a18c897777 190 }
teamat 0:06a18c897777 191
teamat 0:06a18c897777 192 void rightEnd() {
teamat 0:06a18c897777 193 dir=DIR_LEFT;
teamat 0:06a18c897777 194 if (state == STATE_GOTO_END_COUNTING) {
teamat 0:06a18c897777 195 railLength=pos;
teamat 0:06a18c897777 196 state = STATE_GOTO_MIDDLE;
teamat 0:06a18c897777 197 }
teamat 0:06a18c897777 198 else if (isPendulumSwinging()) {
teamat 0:06a18c897777 199 state = STATE_ERROR;
teamat 0:06a18c897777 200 }
teamat 0:06a18c897777 201 }
teamat 0:06a18c897777 202
teamat 0:06a18c897777 203
teamat 0:06a18c897777 204 void sendData() {
teamat 0:06a18c897777 205 myUnion.number[0] = t.read();
teamat 0:06a18c897777 206 myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
teamat 0:06a18c897777 207 myUnion.number[2] = speed;
teamat 0:06a18c897777 208 myUnion.number[3] = angle;
teamat 0:06a18c897777 209 myUnion.number[4] = anSpd;
teamat 0:06a18c897777 210 myUnion.number[5] = control;
teamat 0:06a18c897777 211 for(int i = 0; i < 24; i++) {
teamat 0:06a18c897777 212 rpc.putc(myUnion.numberCh[i]);
teamat 0:06a18c897777 213 }
teamat 0:06a18c897777 214 }
teamat 0:06a18c897777 215
teamat 0:06a18c897777 216
teamat 0:06a18c897777 217 void changeDir() {
teamat 0:06a18c897777 218 if (dir == DIR_LEFT) {
teamat 0:06a18c897777 219 dir = DIR_RIGHT;
teamat 0:06a18c897777 220 } else {
teamat 0:06a18c897777 221 dir = DIR_LEFT;
teamat 0:06a18c897777 222 }
teamat 0:06a18c897777 223 }
teamat 0:06a18c897777 224
teamat 0:06a18c897777 225
teamat 0:06a18c897777 226 void Rx_interrupt() {
teamat 0:06a18c897777 227 data[cmdIndex] = rpc.getc();
teamat 0:06a18c897777 228 if (cmdIndex == 1) {
teamat 0:06a18c897777 229 newData = true;
teamat 0:06a18c897777 230 }
teamat 0:06a18c897777 231 cmdIndex = !cmdIndex;
teamat 0:06a18c897777 232 }
teamat 0:06a18c897777 233
teamat 0:06a18c897777 234 void proceedCommands() {
teamat 0:06a18c897777 235 newData = false;
teamat 0:06a18c897777 236 switch (data[0]) {
teamat 0:06a18c897777 237 case 45:
teamat 0:06a18c897777 238 NVIC_SystemReset();
teamat 0:06a18c897777 239 case 50:
teamat 0:06a18c897777 240 canSend = true;
teamat 0:06a18c897777 241 break;
teamat 0:06a18c897777 242 case 55:
teamat 0:06a18c897777 243 state = STATE_ERROR;
teamat 0:06a18c897777 244 case 60:
teamat 0:06a18c897777 245 led = !led;
teamat 0:06a18c897777 246 state = STATE_GOTO_SWING;
teamat 0:06a18c897777 247 break;
teamat 0:06a18c897777 248 case 65:
teamat 0:06a18c897777 249 if (data[1] == 1) {
teamat 0:06a18c897777 250 dir = DIR_RIGHT;
teamat 0:06a18c897777 251 } else if (data[1] == 10) {
teamat 0:06a18c897777 252 dir = DIR_LEFT;
teamat 0:06a18c897777 253 }
teamat 0:06a18c897777 254 break;
teamat 0:06a18c897777 255 case 70:
teamat 0:06a18c897777 256 period_us = data[1];
teamat 0:06a18c897777 257 calcControl();
teamat 0:06a18c897777 258 break;
teamat 0:06a18c897777 259 default:
teamat 0:06a18c897777 260 break;
teamat 0:06a18c897777 261 }
teamat 0:06a18c897777 262 }
teamat 0:06a18c897777 263
teamat 0:06a18c897777 264
teamat 0:06a18c897777 265 int main() {
teamat 0:06a18c897777 266 RCout = 1;
teamat 0:06a18c897777 267 wait_ms(500);
teamat 0:06a18c897777 268 spi.format(16,2);
teamat 0:06a18c897777 269 spi.frequency(1000000);
teamat 0:06a18c897777 270 t.start();
teamat 0:06a18c897777 271 leftSwitch.rise(&leftEnd);
teamat 0:06a18c897777 272 rightSwitch.rise(&rightEnd);
teamat 0:06a18c897777 273 rpc.attach(&Rx_interrupt, Serial::RxIrq);
teamat 0:06a18c897777 274 for (int i=5; i>0; i--) {
teamat 0:06a18c897777 275 getPendulumAngle();
teamat 0:06a18c897777 276 wait_ms(500);
teamat 0:06a18c897777 277 }
teamat 0:06a18c897777 278 angleOffset= 3.14 - angle;
teamat 0:06a18c897777 279 wait(5);
teamat 0:06a18c897777 280 calcControl();
teamat 0:06a18c897777 281 tick.attach_us(stepperFlip, timer_period_us);
teamat 0:06a18c897777 282 state=STATE_GOTO_START;
teamat 0:06a18c897777 283 dir=DIR_LEFT;
teamat 0:06a18c897777 284 while(1) {
teamat 0:06a18c897777 285 getAngularSpeed();
teamat 0:06a18c897777 286 if (canSend) {
teamat 0:06a18c897777 287 sendData();
teamat 0:06a18c897777 288 }
teamat 0:06a18c897777 289 if (newData) {
teamat 0:06a18c897777 290 proceedCommands();
teamat 0:06a18c897777 291 }
teamat 0:06a18c897777 292 switch(state) {
teamat 0:06a18c897777 293 case STATE_WAITING:
teamat 0:06a18c897777 294 break;
teamat 0:06a18c897777 295 case STATE_GOTO_START:
teamat 0:06a18c897777 296 break;
teamat 0:06a18c897777 297 case STATE_GOTO_END_COUNTING:
teamat 0:06a18c897777 298 break;
teamat 0:06a18c897777 299 case STATE_GOTO_SWING:
teamat 0:06a18c897777 300 speedTicker.attach(calcSpeed, 1);
teamat 0:06a18c897777 301 state = STATE_SWING_LEFT;
teamat 0:06a18c897777 302 break;
teamat 0:06a18c897777 303 case STATE_SWING_LEFT:
teamat 0:06a18c897777 304 break;
teamat 0:06a18c897777 305 case STATE_SWING_RIGHT:
teamat 0:06a18c897777 306 break;
teamat 0:06a18c897777 307 default:
teamat 0:06a18c897777 308 break;
teamat 0:06a18c897777 309 }
teamat 0:06a18c897777 310 }
teamat 0:06a18c897777 311 }