Artem Solomatin
/
pendudu
fsdfds
main.cpp@11:6bbe8da4b16a, 2020-05-22 (annotated)
- Committer:
- teamat
- Date:
- Fri May 22 13:27:01 2020 +0000
- Revision:
- 11:6bbe8da4b16a
- Parent:
- 10:ca38e4d775c8
- Child:
- 12:ee175985ef09
angle not working
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 | 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 | DigitalOut RCout(D10); |
teamat | 3:8708e61475fe | 22 | DigitalOut dir(D9); |
teamat | 3:8708e61475fe | 23 | InterruptIn leftSwitch(D2); |
teamat | 3:8708e61475fe | 24 | InterruptIn rightSwitch(D3); |
teamat | 3:8708e61475fe | 25 | Ticker tick; |
teamat | 4:fd49edfabfb2 | 26 | Ticker speedTicker; |
teamat | 4:fd49edfabfb2 | 27 | Ticker swingTicker; |
teamat | 6:999e8ae7d969 | 28 | Ticker sendDataTicker; |
teamat | 3:8708e61475fe | 29 | DigitalOut myled(LED2); |
teamat | 3:8708e61475fe | 30 | RawSerial rpc(D1,D0,9600); |
teamat | 3:8708e61475fe | 31 | |
teamat | 11:6bbe8da4b16a | 32 | |
teamat | 11:6bbe8da4b16a | 33 | int period_us = 22;//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 | 8:b3ce040fdebc | 57 | float speed = -1; |
teamat | 8:b3ce040fdebc | 58 | float posOffset = 0.0f; |
teamat | 7:ca62dda005d5 | 59 | float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f}; |
teamat | 3:8708e61475fe | 60 | |
teamat | 3:8708e61475fe | 61 | double dAngle = 0.0f; |
teamat | 3:8708e61475fe | 62 | double anSpd = 0.0f; |
teamat | 3:8708e61475fe | 63 | |
teamat | 3:8708e61475fe | 64 | float timeOldPos = 0.0f; |
teamat | 3:8708e61475fe | 65 | float timeStartPos = 0.0f; |
teamat | 3:8708e61475fe | 66 | |
teamat | 4:fd49edfabfb2 | 67 | float angle=0.0f; |
teamat | 3:8708e61475fe | 68 | float angleOffset = 0; |
teamat | 3:8708e61475fe | 69 | |
teamat | 4:fd49edfabfb2 | 70 | float control = 0.0f; |
teamat | 4:fd49edfabfb2 | 71 | |
teamat | 3:8708e61475fe | 72 | double PIPI = 6.28; |
teamat | 3:8708e61475fe | 73 | |
teamat | 3:8708e61475fe | 74 | bool canSend = false; |
teamat | 8:b3ce040fdebc | 75 | bool periodUpdated = false; |
teamat | 8:b3ce040fdebc | 76 | bool calibrated = false; |
teamat | 9:e8c6a414e226 | 77 | bool dirUpdated = false; |
teamat | 3:8708e61475fe | 78 | |
teamat | 11:6bbe8da4b16a | 79 | |
teamat | 11:6bbe8da4b16a | 80 | DigitalOut led(LED1); |
teamat | 11:6bbe8da4b16a | 81 | Timeout flipper; |
teamat | 11:6bbe8da4b16a | 82 | |
teamat | 11:6bbe8da4b16a | 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 | 3:8708e61475fe | 91 | bool isPendulumSwinging() { |
teamat | 3:8708e61475fe | 92 | return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT; |
teamat | 3:8708e61475fe | 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 | 4:fd49edfabfb2 | 170 | void calcSpeed() { |
teamat | 7:ca62dda005d5 | 171 | /*posMap[posCounter] = getPosMM() / 1000; |
teamat | 7:ca62dda005d5 | 172 | if (posCounter == 3) { |
teamat | 7:ca62dda005d5 | 173 | posCounter = 0; |
teamat | 7:ca62dda005d5 | 174 | float pathSum = 0; |
teamat | 7:ca62dda005d5 | 175 | for (int i = 0; i < 3; i++) { |
teamat | 7:ca62dda005d5 | 176 | float pathDiff = posMap[i + 1] - posMap[i]; |
teamat | 7:ca62dda005d5 | 177 | if ((pathDiff) < 0) { |
teamat | 7:ca62dda005d5 | 178 | pathSum += -pathDiff; |
teamat | 7:ca62dda005d5 | 179 | } else { |
teamat | 7:ca62dda005d5 | 180 | pathSum += pathDiff; |
teamat | 7:ca62dda005d5 | 181 | } |
teamat | 7:ca62dda005d5 | 182 | } |
teamat | 7:ca62dda005d5 | 183 | speed = pathSum; |
teamat | 7:ca62dda005d5 | 184 | } else { |
teamat | 7:ca62dda005d5 | 185 | posCounter += 1; |
teamat | 7:ca62dda005d5 | 186 | }*/ |
teamat | 8:b3ce040fdebc | 187 | xPosNew = (getPosMM() - posOffset) / 1000; |
teamat | 4:fd49edfabfb2 | 188 | speed = xPosNew - xPosOld; |
teamat | 8:b3ce040fdebc | 189 | if (dir == DIR_LEFT) { |
teamat | 8:b3ce040fdebc | 190 | speed = -speed; |
teamat | 8:b3ce040fdebc | 191 | } |
teamat | 9:e8c6a414e226 | 192 | //speed = -speed; |
teamat | 8:b3ce040fdebc | 193 | |
teamat | 4:fd49edfabfb2 | 194 | xPosOld = xPosNew; |
teamat | 4:fd49edfabfb2 | 195 | } |
teamat | 4:fd49edfabfb2 | 196 | |
teamat | 4:fd49edfabfb2 | 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 | 3:8708e61475fe | 206 | } |
teamat | 3:8708e61475fe | 207 | |
teamat | 3:8708e61475fe | 208 | |
teamat | 3:8708e61475fe | 209 | |
teamat | 3:8708e61475fe | 210 | void stepperFlip() { |
teamat | 3:8708e61475fe | 211 | if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){ |
teamat | 3:8708e61475fe | 212 | RCout = !RCout; |
teamat | 3:8708e61475fe | 213 | pos += (dir.read() * 2 - 1); |
teamat | 3:8708e61475fe | 214 | } |
teamat | 3:8708e61475fe | 215 | if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) { |
teamat | 8:b3ce040fdebc | 216 | posOffset = 40; |
teamat | 3:8708e61475fe | 217 | state = STATE_WAITING; |
teamat | 3:8708e61475fe | 218 | } |
teamat | 3:8708e61475fe | 219 | if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) { |
teamat | 3:8708e61475fe | 220 | pos += (dir.read() * 2 - 1); |
teamat | 3:8708e61475fe | 221 | RCout = !RCout; |
teamat | 3:8708e61475fe | 222 | } |
teamat | 11:6bbe8da4b16a | 223 | flipper.attach_us(stepperFlip, period_us/2); |
teamat | 3:8708e61475fe | 224 | } |
teamat | 3:8708e61475fe | 225 | |
teamat | 3:8708e61475fe | 226 | |
teamat | 3:8708e61475fe | 227 | void updatePeriod(){ |
teamat | 11:6bbe8da4b16a | 228 | //tick.detach(); |
teamat | 3:8708e61475fe | 229 | tick.attach_us (&stepperFlip, period_us / 2.0f); |
teamat | 3:8708e61475fe | 230 | } |
teamat | 3:8708e61475fe | 231 | |
teamat | 3:8708e61475fe | 232 | void leftEnd() { |
teamat | 3:8708e61475fe | 233 | dir = DIR_RIGHT; |
teamat | 3:8708e61475fe | 234 | if (state == STATE_GOTO_START) { |
teamat | 3:8708e61475fe | 235 | state = STATE_GOTO_END_COUNTING; |
teamat | 3:8708e61475fe | 236 | pos = 0; |
teamat | 3:8708e61475fe | 237 | } |
teamat | 3:8708e61475fe | 238 | else if (isPendulumSwinging()) { |
teamat | 9:e8c6a414e226 | 239 | //state = STATE_GOTO_MIDDLE; |
teamat | 3:8708e61475fe | 240 | //angleOffset -= 0.006191; |
teamat | 9:e8c6a414e226 | 241 | state = STATE_ERROR; |
teamat | 3:8708e61475fe | 242 | } |
teamat | 9:e8c6a414e226 | 243 | if (state == STATE_GOTO_SWING) { |
teamat | 9:e8c6a414e226 | 244 | state = STATE_ERROR; |
teamat | 9:e8c6a414e226 | 245 | } |
teamat | 3:8708e61475fe | 246 | } |
teamat | 3:8708e61475fe | 247 | |
teamat | 3:8708e61475fe | 248 | void rightEnd() { |
teamat | 3:8708e61475fe | 249 | dir=DIR_LEFT; |
teamat | 3:8708e61475fe | 250 | if (state == STATE_GOTO_END_COUNTING) { |
teamat | 3:8708e61475fe | 251 | railLength=pos; |
teamat | 3:8708e61475fe | 252 | state = STATE_GOTO_MIDDLE; |
teamat | 3:8708e61475fe | 253 | } |
teamat | 3:8708e61475fe | 254 | else if (isPendulumSwinging()) { |
teamat | 9:e8c6a414e226 | 255 | //state = STATE_GOTO_MIDDLE; |
teamat | 3:8708e61475fe | 256 | //angleOffset += 0.006191; |
teamat | 9:e8c6a414e226 | 257 | state = STATE_ERROR; |
teamat | 9:e8c6a414e226 | 258 | } |
teamat | 9:e8c6a414e226 | 259 | if (state == STATE_GOTO_SWING) { |
teamat | 9:e8c6a414e226 | 260 | state = STATE_ERROR; |
teamat | 3:8708e61475fe | 261 | } |
teamat | 3:8708e61475fe | 262 | } |
teamat | 3:8708e61475fe | 263 | |
teamat | 9:e8c6a414e226 | 264 | int swingCounter = 0; |
teamat | 3:8708e61475fe | 265 | void getSwingDirectory() { |
teamat | 9:e8c6a414e226 | 266 | swingCounter += 1; |
teamat | 7:ca62dda005d5 | 267 | control = -control; |
teamat | 4:fd49edfabfb2 | 268 | if (dir == DIR_RIGHT) { |
teamat | 3:8708e61475fe | 269 | state = STATE_SWING_RIGHT; |
teamat | 3:8708e61475fe | 270 | dir = DIR_LEFT; |
teamat | 3:8708e61475fe | 271 | return; |
teamat | 3:8708e61475fe | 272 | } |
teamat | 4:fd49edfabfb2 | 273 | if (dir == DIR_LEFT) { |
teamat | 3:8708e61475fe | 274 | state = STATE_SWING_LEFT; |
teamat | 3:8708e61475fe | 275 | dir = DIR_RIGHT; |
teamat | 3:8708e61475fe | 276 | return; |
teamat | 3:8708e61475fe | 277 | } |
teamat | 3:8708e61475fe | 278 | } |
teamat | 3:8708e61475fe | 279 | |
teamat | 3:8708e61475fe | 280 | void sendData() { |
teamat | 3:8708e61475fe | 281 | myUnion.number[0] = t.read(); |
teamat | 8:b3ce040fdebc | 282 | myUnion.number[1] = (getPosMM()- posOffset) / 1000 ; |
teamat | 4:fd49edfabfb2 | 283 | myUnion.number[2] = speed; |
teamat | 4:fd49edfabfb2 | 284 | //myUnion.number[3] = dAngle; |
teamat | 4:fd49edfabfb2 | 285 | myUnion.number[3] = angle; |
teamat | 3:8708e61475fe | 286 | myUnion.number[4] = anSpd; |
teamat | 4:fd49edfabfb2 | 287 | myUnion.number[5] = control; |
teamat | 4:fd49edfabfb2 | 288 | for(int i = 0; i < 24; i++) { |
teamat | 3:8708e61475fe | 289 | rpc.putc(myUnion.numberCh[i]); |
teamat | 3:8708e61475fe | 290 | } |
teamat | 3:8708e61475fe | 291 | } |
teamat | 3:8708e61475fe | 292 | |
teamat | 10:ca38e4d775c8 | 293 | int dirCounter = 0; |
teamat | 10:ca38e4d775c8 | 294 | int perCounter = 0; |
teamat | 10:ca38e4d775c8 | 295 | |
teamat | 10:ca38e4d775c8 | 296 | |
teamat | 3:8708e61475fe | 297 | void Rx_interrupt() { |
teamat | 4:fd49edfabfb2 | 298 | int command = rpc.getc(); |
teamat | 8:b3ce040fdebc | 299 | switch (command) { |
teamat | 8:b3ce040fdebc | 300 | case 50: |
teamat | 8:b3ce040fdebc | 301 | canSend = true; |
teamat | 8:b3ce040fdebc | 302 | break; |
teamat | 8:b3ce040fdebc | 303 | case 60: |
teamat | 8:b3ce040fdebc | 304 | state = STATE_GOTO_SWING; |
teamat | 8:b3ce040fdebc | 305 | speedTicker.attach(calcSpeed, 1); |
teamat | 8:b3ce040fdebc | 306 | break; |
teamat | 8:b3ce040fdebc | 307 | case 65: |
teamat | 11:6bbe8da4b16a | 308 | int direction = rpc.getc(); |
teamat | 11:6bbe8da4b16a | 309 | if (direction <= 0) { |
teamat | 11:6bbe8da4b16a | 310 | if (dir != DIR_RIGHT) { |
teamat | 11:6bbe8da4b16a | 311 | dir = DIR_RIGHT; |
teamat | 9:e8c6a414e226 | 312 | } |
teamat | 11:6bbe8da4b16a | 313 | } else if (direction > 0) { |
teamat | 11:6bbe8da4b16a | 314 | if (dir != DIR_LEFT) { |
teamat | 11:6bbe8da4b16a | 315 | dir = DIR_LEFT; |
teamat | 11:6bbe8da4b16a | 316 | } |
teamat | 8:b3ce040fdebc | 317 | } |
teamat | 8:b3ce040fdebc | 318 | break; |
teamat | 8:b3ce040fdebc | 319 | case 70: |
teamat | 11:6bbe8da4b16a | 320 | /*int newPeriod = rpc.getc(); |
teamat | 11:6bbe8da4b16a | 321 | if (newPeriod < 22) { |
teamat | 11:6bbe8da4b16a | 322 | flipper.detach(); |
teamat | 11:6bbe8da4b16a | 323 | } else if (period_us != newPeriod) { |
teamat | 11:6bbe8da4b16a | 324 | period_us = newPeriod; |
teamat | 11:6bbe8da4b16a | 325 | periodUpdated = true; |
teamat | 11:6bbe8da4b16a | 326 | }*/ |
teamat | 8:b3ce040fdebc | 327 | break; |
teamat | 8:b3ce040fdebc | 328 | default: |
teamat | 8:b3ce040fdebc | 329 | break; |
teamat | 3:8708e61475fe | 330 | } |
teamat | 3:8708e61475fe | 331 | return; |
teamat | 3:8708e61475fe | 332 | } |
teamat | 3:8708e61475fe | 333 | |
teamat | 11:6bbe8da4b16a | 334 | /*void flipLed() |
teamat | 11:6bbe8da4b16a | 335 | { |
teamat | 11:6bbe8da4b16a | 336 | flipper.attach_us(stepperFlip, period_us); |
teamat | 11:6bbe8da4b16a | 337 | }*/ |
teamat | 11:6bbe8da4b16a | 338 | |
teamat | 3:8708e61475fe | 339 | int main() { |
teamat | 3:8708e61475fe | 340 | RCout = 1; |
teamat | 3:8708e61475fe | 341 | wait_ms(500); |
teamat | 3:8708e61475fe | 342 | spi.format(16,2); |
teamat | 3:8708e61475fe | 343 | spi.frequency(1000000); |
teamat | 3:8708e61475fe | 344 | t.start(); |
teamat | 3:8708e61475fe | 345 | leftSwitch.rise(&leftEnd); |
teamat | 3:8708e61475fe | 346 | rightSwitch.rise(&rightEnd); |
teamat | 4:fd49edfabfb2 | 347 | rpc.attach(&Rx_interrupt, Serial::RxIrq); |
teamat | 3:8708e61475fe | 348 | for (int i=5; i>0; i--) { |
teamat | 3:8708e61475fe | 349 | getPendulumAngle(); |
teamat | 3:8708e61475fe | 350 | wait_ms(500); |
teamat | 3:8708e61475fe | 351 | } |
teamat | 4:fd49edfabfb2 | 352 | angleOffset= 3.14 - angle; |
teamat | 8:b3ce040fdebc | 353 | calibrated = true; |
teamat | 8:b3ce040fdebc | 354 | //wait(12); |
teamat | 8:b3ce040fdebc | 355 | wait(7); |
teamat | 11:6bbe8da4b16a | 356 | //updatePeriod(); |
teamat | 6:999e8ae7d969 | 357 | calcControl(); |
teamat | 3:8708e61475fe | 358 | state=STATE_GOTO_START; |
teamat | 3:8708e61475fe | 359 | dir=DIR_LEFT; |
teamat | 11:6bbe8da4b16a | 360 | flipper.attach_us(stepperFlip, period_us/2); |
teamat | 3:8708e61475fe | 361 | while(1) { |
teamat | 4:fd49edfabfb2 | 362 | getAngularSpeed(); |
teamat | 8:b3ce040fdebc | 363 | if (canSend) { |
teamat | 3:8708e61475fe | 364 | sendData(); |
teamat | 3:8708e61475fe | 365 | } |
teamat | 11:6bbe8da4b16a | 366 | if (periodUpdated) { |
teamat | 8:b3ce040fdebc | 367 | calcControl(); |
teamat | 11:6bbe8da4b16a | 368 | // updatePeriod(); |
teamat | 11:6bbe8da4b16a | 369 | //flipper.attach_us(stepperFlip, period_us/2); |
teamat | 8:b3ce040fdebc | 370 | periodUpdated = false; |
teamat | 11:6bbe8da4b16a | 371 | } |
teamat | 3:8708e61475fe | 372 | switch(state) { |
teamat | 3:8708e61475fe | 373 | case STATE_WAITING: |
teamat | 8:b3ce040fdebc | 374 | //state = STATE_GOTO_SWING; |
teamat | 3:8708e61475fe | 375 | break; |
teamat | 3:8708e61475fe | 376 | case STATE_GOTO_START: |
teamat | 3:8708e61475fe | 377 | break; |
teamat | 3:8708e61475fe | 378 | case STATE_GOTO_END_COUNTING: |
teamat | 3:8708e61475fe | 379 | break; |
teamat | 3:8708e61475fe | 380 | case STATE_GOTO_SWING: |
teamat | 9:e8c6a414e226 | 381 | //speedTicker.attach(calcSpeed, 1); |
teamat | 8:b3ce040fdebc | 382 | //swingTicker.attach(getSwingDirectory, 1); |
teamat | 8:b3ce040fdebc | 383 | //state = STATE_SWING_LEFT; |
teamat | 3:8708e61475fe | 384 | break; |
teamat | 3:8708e61475fe | 385 | case STATE_SWING_LEFT: |
teamat | 9:e8c6a414e226 | 386 | /*if (swingCounter == 6) { |
teamat | 9:e8c6a414e226 | 387 | swingCounter = 0; |
teamat | 9:e8c6a414e226 | 388 | period_us += 4; |
teamat | 9:e8c6a414e226 | 389 | periodUpdated = true; |
teamat | 9:e8c6a414e226 | 390 | }*/ |
teamat | 3:8708e61475fe | 391 | break; |
teamat | 3:8708e61475fe | 392 | case STATE_SWING_RIGHT: |
teamat | 9:e8c6a414e226 | 393 | /*if (swingCounter == 6) { |
teamat | 9:e8c6a414e226 | 394 | swingCounter = 0; |
teamat | 9:e8c6a414e226 | 395 | period_us += 4; |
teamat | 9:e8c6a414e226 | 396 | periodUpdated = true; |
teamat | 9:e8c6a414e226 | 397 | }*/ |
teamat | 3:8708e61475fe | 398 | break; |
teamat | 3:8708e61475fe | 399 | default: |
teamat | 3:8708e61475fe | 400 | break; |
teamat | 3:8708e61475fe | 401 | } |
teamat | 10:ca38e4d775c8 | 402 | //wait_ms(100); |
teamat | 3:8708e61475fe | 403 | } |
teamat | 3:8708e61475fe | 404 | } |