Artem Solomatin
/
pendudu
fsdfds
main.cpp@13:205002e3c176, 2020-05-24 (annotated)
- Committer:
- teamat
- Date:
- Sun May 24 13:23:08 2020 +0000
- Revision:
- 13:205002e3c176
- Parent:
- 12:ee175985ef09
- Child:
- 14:353e92037bc8
ready for experiments
Who changed what in which revision?
User | Revision | Line number | New 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 | 12:ee175985ef09 | 6 | |
teamat | 3:8708e61475fe | 7 | #define STATE_ERROR 0 |
teamat | 3:8708e61475fe | 8 | #define STATE_INIT 1 |
teamat | 3:8708e61475fe | 9 | #define STATE_GOTO_START 2 |
teamat | 3:8708e61475fe | 10 | #define STATE_GOTO_END_COUNTING 3 |
teamat | 3:8708e61475fe | 11 | #define STATE_GOTO_MIDDLE 4 |
teamat | 3:8708e61475fe | 12 | #define STATE_WAITING 5 |
teamat | 3:8708e61475fe | 13 | #define STATE_GOTO_SWING 6 |
teamat | 3:8708e61475fe | 14 | #define STATE_START_SWING 7 |
teamat | 3:8708e61475fe | 15 | #define STATE_SWING_RIGHT 8 |
teamat | 3:8708e61475fe | 16 | #define STATE_SWING_LEFT 9 |
teamat | 3:8708e61475fe | 17 | #define STATE_EMPTY_SWING 10 |
teamat | 3:8708e61475fe | 18 | |
teamat | 3:8708e61475fe | 19 | #define DIR_LEFT 0 |
teamat | 3:8708e61475fe | 20 | #define DIR_RIGHT 1 |
teamat | 3:8708e61475fe | 21 | |
teamat | 12:ee175985ef09 | 22 | #define timer_period_us 10 |
teamat | 12:ee175985ef09 | 23 | |
teamat | 12:ee175985ef09 | 24 | |
teamat | 3:8708e61475fe | 25 | DigitalOut RCout(D10); |
teamat | 3:8708e61475fe | 26 | DigitalOut dir(D9); |
teamat | 3:8708e61475fe | 27 | InterruptIn leftSwitch(D2); |
teamat | 3:8708e61475fe | 28 | InterruptIn rightSwitch(D3); |
teamat | 3:8708e61475fe | 29 | Ticker tick; |
teamat | 13:205002e3c176 | 30 | Ticker speedTicker; |
teamat | 4:fd49edfabfb2 | 31 | Ticker swingTicker; |
teamat | 3:8708e61475fe | 32 | RawSerial rpc(D1,D0,9600); |
teamat | 12:ee175985ef09 | 33 | DigitalOut led(D4); |
teamat | 11:6bbe8da4b16a | 34 | |
teamat | 12:ee175985ef09 | 35 | int period_us = 1;//300 26 |
teamat | 7:ca62dda005d5 | 36 | int posCounter = 0; |
teamat | 12:ee175985ef09 | 37 | int timerCounter = 0; |
teamat | 3:8708e61475fe | 38 | |
teamat | 3:8708e61475fe | 39 | long pos=0; |
teamat | 3:8708e61475fe | 40 | |
teamat | 3:8708e61475fe | 41 | long railLength = 0; |
teamat | 3:8708e61475fe | 42 | |
teamat | 3:8708e61475fe | 43 | uint8_t state=STATE_INIT; |
teamat | 12:ee175985ef09 | 44 | uint8_t data[2] = {0, 0}; |
teamat | 12:ee175985ef09 | 45 | uint8_t cmdIndex = 0; |
teamat | 3:8708e61475fe | 46 | |
teamat | 3:8708e61475fe | 47 | Timer t; |
teamat | 3:8708e61475fe | 48 | SPI spi(D11,D12,D13);// mosi, miso, sclk |
teamat | 3:8708e61475fe | 49 | DigitalOut cs(D5); |
teamat | 3:8708e61475fe | 50 | |
teamat | 4:fd49edfabfb2 | 51 | float radius = 0.0025; |
teamat | 3:8708e61475fe | 52 | |
teamat | 3:8708e61475fe | 53 | float angularPosNew = 0; |
teamat | 3:8708e61475fe | 54 | float angularPosOld = 0; |
teamat | 3:8708e61475fe | 55 | |
teamat | 3:8708e61475fe | 56 | float timeOld = 0.0f; |
teamat | 3:8708e61475fe | 57 | float timeStart = 0.0f; |
teamat | 3:8708e61475fe | 58 | |
teamat | 3:8708e61475fe | 59 | float dx = 0; |
teamat | 3:8708e61475fe | 60 | float xPosNew = 0; |
teamat | 3:8708e61475fe | 61 | float xPosOld = 0; |
teamat | 8:b3ce040fdebc | 62 | float speed = -1; |
teamat | 8:b3ce040fdebc | 63 | float posOffset = 0.0f; |
teamat | 7:ca62dda005d5 | 64 | float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f}; |
teamat | 3:8708e61475fe | 65 | |
teamat | 3:8708e61475fe | 66 | double dAngle = 0.0f; |
teamat | 3:8708e61475fe | 67 | double anSpd = 0.0f; |
teamat | 3:8708e61475fe | 68 | |
teamat | 3:8708e61475fe | 69 | float timeOldPos = 0.0f; |
teamat | 3:8708e61475fe | 70 | float timeStartPos = 0.0f; |
teamat | 3:8708e61475fe | 71 | |
teamat | 4:fd49edfabfb2 | 72 | float angle=0.0f; |
teamat | 3:8708e61475fe | 73 | float angleOffset = 0; |
teamat | 3:8708e61475fe | 74 | |
teamat | 4:fd49edfabfb2 | 75 | float control = 0.0f; |
teamat | 4:fd49edfabfb2 | 76 | |
teamat | 3:8708e61475fe | 77 | double PIPI = 6.28; |
teamat | 3:8708e61475fe | 78 | |
teamat | 3:8708e61475fe | 79 | bool canSend = false; |
teamat | 8:b3ce040fdebc | 80 | bool periodUpdated = false; |
teamat | 8:b3ce040fdebc | 81 | bool calibrated = false; |
teamat | 9:e8c6a414e226 | 82 | bool dirUpdated = false; |
teamat | 3:8708e61475fe | 83 | |
teamat | 3:8708e61475fe | 84 | typedef union { |
teamat | 4:fd49edfabfb2 | 85 | float number[6]; |
teamat | 4:fd49edfabfb2 | 86 | uint8_t numberCh[24]; |
teamat | 3:8708e61475fe | 87 | } my_union; |
teamat | 3:8708e61475fe | 88 | |
teamat | 3:8708e61475fe | 89 | my_union myUnion; |
teamat | 3:8708e61475fe | 90 | |
teamat | 13:205002e3c176 | 91 | bool isPendulumSwinging() { |
teamat | 3:8708e61475fe | 92 | return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT; |
teamat | 13:205002e3c176 | 93 | } |
teamat | 3:8708e61475fe | 94 | |
teamat | 3:8708e61475fe | 95 | float getPosMM() { |
teamat | 3:8708e61475fe | 96 | //return (pos-railLength/2) * 550.0f/railLength; |
teamat | 3:8708e61475fe | 97 | //return (pos - railLength / 2) * (350.0f / railLength); |
teamat | 3:8708e61475fe | 98 | return pos / 3500.0f; |
teamat | 3:8708e61475fe | 99 | } |
teamat | 3:8708e61475fe | 100 | |
teamat | 3:8708e61475fe | 101 | uint16_t getPendulumPos(){ |
teamat | 3:8708e61475fe | 102 | cs=0; |
teamat | 3:8708e61475fe | 103 | wait_ms(1); |
teamat | 3:8708e61475fe | 104 | uint16_t d=spi.write((short)0x00); |
teamat | 3:8708e61475fe | 105 | d=d<<1;//fucking shithole fakebit |
teamat | 3:8708e61475fe | 106 | d=d>>6;//no need debug info |
teamat | 3:8708e61475fe | 107 | cs=1; |
teamat | 3:8708e61475fe | 108 | wait_ms(1); |
teamat | 3:8708e61475fe | 109 | return (uint16_t)d; |
teamat | 3:8708e61475fe | 110 | } |
teamat | 3:8708e61475fe | 111 | |
teamat | 3:8708e61475fe | 112 | |
teamat | 3:8708e61475fe | 113 | float getPendulumAngle(){ |
teamat | 3:8708e61475fe | 114 | angle = getPendulumPos(); |
teamat | 3:8708e61475fe | 115 | angle = angle * 6.28f / 1024.0f; |
teamat | 4:fd49edfabfb2 | 116 | angle += angleOffset; |
teamat | 8:b3ce040fdebc | 117 | if (angle > 3.14) { |
teamat | 8:b3ce040fdebc | 118 | angle = angle - PIPI; |
teamat | 8:b3ce040fdebc | 119 | } |
teamat | 8:b3ce040fdebc | 120 | /*if (angle > PIPI + 0.01) { |
teamat | 4:fd49edfabfb2 | 121 | angle = fmod((angle + 3.14), PIPI) - 3.14; |
teamat | 4:fd49edfabfb2 | 122 | if (angle < -3.14) { |
teamat | 4:fd49edfabfb2 | 123 | angle += PIPI; |
teamat | 4:fd49edfabfb2 | 124 | } |
teamat | 9:e8c6a414e226 | 125 | }*/ |
teamat | 8:b3ce040fdebc | 126 | /*if (calibrated) { |
teamat | 8:b3ce040fdebc | 127 | float test = fmodf(angle, M_PI); |
teamat | 8:b3ce040fdebc | 128 | if (test >= 1) { |
teamat | 8:b3ce040fdebc | 129 | angle = -test; |
teamat | 8:b3ce040fdebc | 130 | } else { |
teamat | 8:b3ce040fdebc | 131 | angle = test; |
teamat | 8:b3ce040fdebc | 132 | } |
teamat | 8:b3ce040fdebc | 133 | }*/ |
teamat | 3:8708e61475fe | 134 | return angle; |
teamat | 3:8708e61475fe | 135 | } |
teamat | 3:8708e61475fe | 136 | |
teamat | 3:8708e61475fe | 137 | |
teamat | 3:8708e61475fe | 138 | /* |
teamat | 3:8708e61475fe | 139 | ANGULAR SPEED CALC |
teamat | 3:8708e61475fe | 140 | */ |
teamat | 3:8708e61475fe | 141 | |
teamat | 3:8708e61475fe | 142 | void getDeltaAng(){ |
teamat | 3:8708e61475fe | 143 | |
teamat | 3:8708e61475fe | 144 | angularPosNew = getPendulumAngle(); |
teamat | 3:8708e61475fe | 145 | |
teamat | 3:8708e61475fe | 146 | dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14; |
teamat | 3:8708e61475fe | 147 | if (dAngle < -3.14) { |
teamat | 3:8708e61475fe | 148 | dAngle += PIPI; |
teamat | 3:8708e61475fe | 149 | } |
teamat | 3:8708e61475fe | 150 | |
teamat | 3:8708e61475fe | 151 | angularPosOld = angularPosNew; |
teamat | 3:8708e61475fe | 152 | |
teamat | 3:8708e61475fe | 153 | } |
teamat | 3:8708e61475fe | 154 | |
teamat | 3:8708e61475fe | 155 | void getAngularSpeed(){ |
teamat | 3:8708e61475fe | 156 | float deltaTime; |
teamat | 3:8708e61475fe | 157 | |
teamat | 3:8708e61475fe | 158 | timeStart = float(t.read()); |
teamat | 3:8708e61475fe | 159 | deltaTime = (timeStart - timeOld); |
teamat | 3:8708e61475fe | 160 | getDeltaAng(); |
teamat | 3:8708e61475fe | 161 | anSpd = dAngle / deltaTime; |
teamat | 3:8708e61475fe | 162 | timeOld=timeStart; |
teamat | 3:8708e61475fe | 163 | //взятие по модулю, спросить |
teamat | 3:8708e61475fe | 164 | } |
teamat | 3:8708e61475fe | 165 | |
teamat | 3:8708e61475fe | 166 | /* |
teamat | 3:8708e61475fe | 167 | SPEED CALC |
teamat | 3:8708e61475fe | 168 | */ |
teamat | 3:8708e61475fe | 169 | |
teamat | 13:205002e3c176 | 170 | |
teamat | 13:205002e3c176 | 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 | 13:205002e3c176 | 188 | |
teamat | 13:205002e3c176 | 189 | xPosNew = (getPosMM() - posOffset) / 1000; |
teamat | 13:205002e3c176 | 190 | speed = xPosNew - xPosOld; |
teamat | 13:205002e3c176 | 191 | if (dir == DIR_LEFT) { |
teamat | 13:205002e3c176 | 192 | speed = -speed; |
teamat | 13:205002e3c176 | 193 | } |
teamat | 13:205002e3c176 | 194 | xPosOld = xPosNew; |
teamat | 13:205002e3c176 | 195 | } |
teamat | 4:fd49edfabfb2 | 196 | |
teamat | 13:205002e3c176 | 197 | void calcControl() { |
teamat | 6:999e8ae7d969 | 198 | //float frequency = 1000000 / (period_us / 2); |
teamat | 6:999e8ae7d969 | 199 | //float rates = frequency / 6400; |
teamat | 6:999e8ae7d969 | 200 | //control = rates * PIPI * radius; |
teamat | 8:b3ce040fdebc | 201 | if (dir = DIR_RIGHT) { |
teamat | 8:b3ce040fdebc | 202 | control = -period_us; |
teamat | 8:b3ce040fdebc | 203 | } else { |
teamat | 8:b3ce040fdebc | 204 | control = period_us; |
teamat | 8:b3ce040fdebc | 205 | } |
teamat | 13:205002e3c176 | 206 | } |
teamat | 3:8708e61475fe | 207 | |
teamat | 3:8708e61475fe | 208 | |
teamat | 3:8708e61475fe | 209 | |
teamat | 3:8708e61475fe | 210 | void stepperFlip() { |
teamat | 12:ee175985ef09 | 211 | if (timerCounter * timer_period_us > period_us) { |
teamat | 12:ee175985ef09 | 212 | timerCounter = 0; |
teamat | 12:ee175985ef09 | 213 | if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){ |
teamat | 12:ee175985ef09 | 214 | RCout = !RCout; |
teamat | 12:ee175985ef09 | 215 | pos += (dir.read() * 2 - 1); |
teamat | 12:ee175985ef09 | 216 | } |
teamat | 12:ee175985ef09 | 217 | if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) { |
teamat | 12:ee175985ef09 | 218 | posOffset = 40; |
teamat | 12:ee175985ef09 | 219 | state = STATE_WAITING; |
teamat | 12:ee175985ef09 | 220 | } |
teamat | 12:ee175985ef09 | 221 | if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) { |
teamat | 12:ee175985ef09 | 222 | pos += (dir.read() * 2 - 1); |
teamat | 12:ee175985ef09 | 223 | RCout = !RCout; |
teamat | 12:ee175985ef09 | 224 | } |
teamat | 12:ee175985ef09 | 225 | } else { |
teamat | 12:ee175985ef09 | 226 | timerCounter += 1; |
teamat | 3:8708e61475fe | 227 | } |
teamat | 3:8708e61475fe | 228 | } |
teamat | 3:8708e61475fe | 229 | |
teamat | 3:8708e61475fe | 230 | |
teamat | 12:ee175985ef09 | 231 | /*void updatePeriod(){ |
teamat | 12:ee175985ef09 | 232 | tick.detach(); |
teamat | 3:8708e61475fe | 233 | tick.attach_us (&stepperFlip, period_us / 2.0f); |
teamat | 12:ee175985ef09 | 234 | wait_ms(20); |
teamat | 12:ee175985ef09 | 235 | }*/ |
teamat | 3:8708e61475fe | 236 | |
teamat | 3:8708e61475fe | 237 | void leftEnd() { |
teamat | 3:8708e61475fe | 238 | dir = DIR_RIGHT; |
teamat | 3:8708e61475fe | 239 | if (state == STATE_GOTO_START) { |
teamat | 3:8708e61475fe | 240 | state = STATE_GOTO_END_COUNTING; |
teamat | 3:8708e61475fe | 241 | pos = 0; |
teamat | 3:8708e61475fe | 242 | } |
teamat | 13:205002e3c176 | 243 | else if (isPendulumSwinging()) { |
teamat | 9:e8c6a414e226 | 244 | //state = STATE_GOTO_MIDDLE; |
teamat | 3:8708e61475fe | 245 | //angleOffset -= 0.006191; |
teamat | 9:e8c6a414e226 | 246 | state = STATE_ERROR; |
teamat | 13:205002e3c176 | 247 | } |
teamat | 13:205002e3c176 | 248 | /*if (state == STATE_GOTO_SWING) { |
teamat | 13:205002e3c176 | 249 | state = STATE_ERROR; |
teamat | 12:ee175985ef09 | 250 | }*/ |
teamat | 3:8708e61475fe | 251 | } |
teamat | 3:8708e61475fe | 252 | |
teamat | 3:8708e61475fe | 253 | void rightEnd() { |
teamat | 3:8708e61475fe | 254 | dir=DIR_LEFT; |
teamat | 3:8708e61475fe | 255 | if (state == STATE_GOTO_END_COUNTING) { |
teamat | 3:8708e61475fe | 256 | railLength=pos; |
teamat | 3:8708e61475fe | 257 | state = STATE_GOTO_MIDDLE; |
teamat | 3:8708e61475fe | 258 | } |
teamat | 13:205002e3c176 | 259 | else if (isPendulumSwinging()) { |
teamat | 9:e8c6a414e226 | 260 | //state = STATE_GOTO_MIDDLE; |
teamat | 3:8708e61475fe | 261 | //angleOffset += 0.006191; |
teamat | 9:e8c6a414e226 | 262 | state = STATE_ERROR; |
teamat | 13:205002e3c176 | 263 | } |
teamat | 13:205002e3c176 | 264 | /*if (state == STATE_GOTO_SWING) { |
teamat | 13:205002e3c176 | 265 | state = STATE_ERROR; |
teamat | 12:ee175985ef09 | 266 | }*/ |
teamat | 3:8708e61475fe | 267 | } |
teamat | 3:8708e61475fe | 268 | |
teamat | 3:8708e61475fe | 269 | |
teamat | 3:8708e61475fe | 270 | void sendData() { |
teamat | 3:8708e61475fe | 271 | myUnion.number[0] = t.read(); |
teamat | 8:b3ce040fdebc | 272 | myUnion.number[1] = (getPosMM()- posOffset) / 1000 ; |
teamat | 4:fd49edfabfb2 | 273 | myUnion.number[2] = speed; |
teamat | 4:fd49edfabfb2 | 274 | //myUnion.number[3] = dAngle; |
teamat | 4:fd49edfabfb2 | 275 | myUnion.number[3] = angle; |
teamat | 3:8708e61475fe | 276 | myUnion.number[4] = anSpd; |
teamat | 4:fd49edfabfb2 | 277 | myUnion.number[5] = control; |
teamat | 4:fd49edfabfb2 | 278 | for(int i = 0; i < 24; i++) { |
teamat | 3:8708e61475fe | 279 | rpc.putc(myUnion.numberCh[i]); |
teamat | 3:8708e61475fe | 280 | } |
teamat | 3:8708e61475fe | 281 | } |
teamat | 3:8708e61475fe | 282 | |
teamat | 10:ca38e4d775c8 | 283 | |
teamat | 12:ee175985ef09 | 284 | void changeDir() { |
teamat | 12:ee175985ef09 | 285 | if (dir == DIR_LEFT) { |
teamat | 12:ee175985ef09 | 286 | dir = DIR_RIGHT; |
teamat | 12:ee175985ef09 | 287 | } else { |
teamat | 12:ee175985ef09 | 288 | dir = DIR_LEFT; |
teamat | 12:ee175985ef09 | 289 | } |
teamat | 12:ee175985ef09 | 290 | } |
teamat | 12:ee175985ef09 | 291 | |
teamat | 12:ee175985ef09 | 292 | int command[2] = {0, 0}; |
teamat | 12:ee175985ef09 | 293 | bool newData = false; |
teamat | 10:ca38e4d775c8 | 294 | |
teamat | 3:8708e61475fe | 295 | void Rx_interrupt() { |
teamat | 12:ee175985ef09 | 296 | data[cmdIndex] = rpc.getc(); |
teamat | 12:ee175985ef09 | 297 | if (cmdIndex == 1) { |
teamat | 12:ee175985ef09 | 298 | newData = true; |
teamat | 12:ee175985ef09 | 299 | } |
teamat | 12:ee175985ef09 | 300 | cmdIndex = !cmdIndex; |
teamat | 3:8708e61475fe | 301 | } |
teamat | 3:8708e61475fe | 302 | |
teamat | 12:ee175985ef09 | 303 | void proceedCommands() { |
teamat | 12:ee175985ef09 | 304 | newData = false; |
teamat | 12:ee175985ef09 | 305 | switch (data[0]) { |
teamat | 12:ee175985ef09 | 306 | case 50: |
teamat | 12:ee175985ef09 | 307 | canSend = true; |
teamat | 12:ee175985ef09 | 308 | break; |
teamat | 12:ee175985ef09 | 309 | case 60: |
teamat | 13:205002e3c176 | 310 | led = !led; |
teamat | 13:205002e3c176 | 311 | state = STATE_GOTO_SWING; |
teamat | 13:205002e3c176 | 312 | break; |
teamat | 13:205002e3c176 | 313 | case 65: |
teamat | 13:205002e3c176 | 314 | led = !led; |
teamat | 13:205002e3c176 | 315 | if (data[1] == 1) { |
teamat | 13:205002e3c176 | 316 | dir = DIR_RIGHT; |
teamat | 13:205002e3c176 | 317 | } else if (data[1] == 10) { |
teamat | 13:205002e3c176 | 318 | dir = DIR_LEFT; |
teamat | 13:205002e3c176 | 319 | } |
teamat | 12:ee175985ef09 | 320 | break; |
teamat | 12:ee175985ef09 | 321 | case 70: |
teamat | 12:ee175985ef09 | 322 | period_us = data[1]; |
teamat | 13:205002e3c176 | 323 | calcControl(); |
teamat | 12:ee175985ef09 | 324 | break; |
teamat | 12:ee175985ef09 | 325 | default: |
teamat | 12:ee175985ef09 | 326 | break; |
teamat | 12:ee175985ef09 | 327 | } |
teamat | 12:ee175985ef09 | 328 | } |
teamat | 12:ee175985ef09 | 329 | |
teamat | 12:ee175985ef09 | 330 | |
teamat | 3:8708e61475fe | 331 | int main() { |
teamat | 3:8708e61475fe | 332 | RCout = 1; |
teamat | 3:8708e61475fe | 333 | wait_ms(500); |
teamat | 3:8708e61475fe | 334 | spi.format(16,2); |
teamat | 3:8708e61475fe | 335 | spi.frequency(1000000); |
teamat | 3:8708e61475fe | 336 | t.start(); |
teamat | 3:8708e61475fe | 337 | leftSwitch.rise(&leftEnd); |
teamat | 3:8708e61475fe | 338 | rightSwitch.rise(&rightEnd); |
teamat | 4:fd49edfabfb2 | 339 | rpc.attach(&Rx_interrupt, Serial::RxIrq); |
teamat | 3:8708e61475fe | 340 | for (int i=5; i>0; i--) { |
teamat | 3:8708e61475fe | 341 | getPendulumAngle(); |
teamat | 3:8708e61475fe | 342 | wait_ms(500); |
teamat | 3:8708e61475fe | 343 | } |
teamat | 4:fd49edfabfb2 | 344 | angleOffset= 3.14 - angle; |
teamat | 8:b3ce040fdebc | 345 | calibrated = true; |
teamat | 13:205002e3c176 | 346 | led = !led; |
teamat | 8:b3ce040fdebc | 347 | //wait(12); |
teamat | 12:ee175985ef09 | 348 | //wait(3); |
teamat | 11:6bbe8da4b16a | 349 | //updatePeriod(); |
teamat | 13:205002e3c176 | 350 | calcControl(); |
teamat | 12:ee175985ef09 | 351 | tick.attach_us(stepperFlip, timer_period_us); |
teamat | 3:8708e61475fe | 352 | state=STATE_GOTO_START; |
teamat | 3:8708e61475fe | 353 | dir=DIR_LEFT; |
teamat | 13:205002e3c176 | 354 | //swingTicker.attach(changeDir, 1.5); |
teamat | 3:8708e61475fe | 355 | while(1) { |
teamat | 4:fd49edfabfb2 | 356 | getAngularSpeed(); |
teamat | 8:b3ce040fdebc | 357 | if (canSend) { |
teamat | 3:8708e61475fe | 358 | sendData(); |
teamat | 3:8708e61475fe | 359 | } |
teamat | 12:ee175985ef09 | 360 | if (newData) { |
teamat | 12:ee175985ef09 | 361 | proceedCommands(); |
teamat | 12:ee175985ef09 | 362 | } |
teamat | 12:ee175985ef09 | 363 | /*if (periodUpdated) { |
teamat | 8:b3ce040fdebc | 364 | calcControl(); |
teamat | 12:ee175985ef09 | 365 | updatePeriod(); |
teamat | 8:b3ce040fdebc | 366 | periodUpdated = false; |
teamat | 12:ee175985ef09 | 367 | }*/ |
teamat | 3:8708e61475fe | 368 | switch(state) { |
teamat | 3:8708e61475fe | 369 | case STATE_WAITING: |
teamat | 8:b3ce040fdebc | 370 | //state = STATE_GOTO_SWING; |
teamat | 3:8708e61475fe | 371 | break; |
teamat | 3:8708e61475fe | 372 | case STATE_GOTO_START: |
teamat | 3:8708e61475fe | 373 | break; |
teamat | 3:8708e61475fe | 374 | case STATE_GOTO_END_COUNTING: |
teamat | 3:8708e61475fe | 375 | break; |
teamat | 3:8708e61475fe | 376 | case STATE_GOTO_SWING: |
teamat | 13:205002e3c176 | 377 | speedTicker.attach(calcSpeed, 1); |
teamat | 8:b3ce040fdebc | 378 | //swingTicker.attach(getSwingDirectory, 1); |
teamat | 13:205002e3c176 | 379 | state = STATE_SWING_LEFT; |
teamat | 3:8708e61475fe | 380 | break; |
teamat | 3:8708e61475fe | 381 | case STATE_SWING_LEFT: |
teamat | 9:e8c6a414e226 | 382 | /*if (swingCounter == 6) { |
teamat | 9:e8c6a414e226 | 383 | swingCounter = 0; |
teamat | 9:e8c6a414e226 | 384 | period_us += 4; |
teamat | 9:e8c6a414e226 | 385 | periodUpdated = true; |
teamat | 9:e8c6a414e226 | 386 | }*/ |
teamat | 3:8708e61475fe | 387 | break; |
teamat | 3:8708e61475fe | 388 | case STATE_SWING_RIGHT: |
teamat | 9:e8c6a414e226 | 389 | /*if (swingCounter == 6) { |
teamat | 9:e8c6a414e226 | 390 | swingCounter = 0; |
teamat | 9:e8c6a414e226 | 391 | period_us += 4; |
teamat | 9:e8c6a414e226 | 392 | periodUpdated = true; |
teamat | 9:e8c6a414e226 | 393 | }*/ |
teamat | 3:8708e61475fe | 394 | break; |
teamat | 3:8708e61475fe | 395 | default: |
teamat | 3:8708e61475fe | 396 | break; |
teamat | 3:8708e61475fe | 397 | } |
teamat | 3:8708e61475fe | 398 | } |
teamat | 3:8708e61475fe | 399 | } |