Artem Solomatin / Mbed 2 deprecated pendulum

fsdfds

Dependencies:   mbed

main.cpp@3:8708e61475fe, 2020-05-16 (annotated)

Committer:
teamat
Date:
Sat May 16 12:31:09 2020 +0000
Revision:
3:8708e61475fe
Parent:
2:32c9df18a589 
fixi

Who changed what in which revision?

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