Artem Solomatin / Mbed 2 deprecated pendudu

fsdfds

Dependencies:   mbed

main.cpp@7:ca62dda005d5, 2020-05-19 (annotated)

Committer:
teamat
Date:
Tue May 19 14:31:15 2020 +0000
Revision:
7:ca62dda005d5
Parent:
6:999e8ae7d969
Child:
8:b3ce040fdebc
Fix speed and control

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