Rauno U
/
Miisu
Six crescent shaped legs
main.cpp@44:c2acf8d5e191, 2016-06-20 (annotated)
- Committer:
- phairero
- Date:
- Mon Jun 20 15:47:07 2016 +0000
- Revision:
- 44:c2acf8d5e191
- Parent:
- 43:0627a2245b9d
- Child:
- 45:409988ab53e4
Moving and turning. Tested on ground. RF messes up sometimes.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
phairero | 0:0b7259fdb68a | 1 | #include "mbed.h" |
sim642 | 3:2235787e78c4 | 2 | #include "EncoderMotor.hpp" |
phairero | 0:0b7259fdb68a | 3 | |
phairero | 0:0b7259fdb68a | 4 | InterruptIn bt(USER_BUTTON); |
phairero | 30:476ab400d765 | 5 | Serial pc(PA_0, PA_1); //RF |
phairero | 30:476ab400d765 | 6 | //Serial pc(SERIAL_TX, SERIAL_RX); |
phairero | 26:c865244ca3cf | 7 | //Serial pc(USBTX, USBRX); |
sim642 | 2:cf0147952fb9 | 8 | |
sim642 | 23:d844cc906b66 | 9 | //PIDData speedPIDData = {0.3f, 2.0f, 0.02f}; |
sim642 | 23:d844cc906b66 | 10 | //PIDData turnPIDData = {5.0f, 0.1f, 0.04f}; |
sim642 | 23:d844cc906b66 | 11 | PIDData speedPIDData = {0.5f, 0.0f, 0.0f}; |
phairero | 26:c865244ca3cf | 12 | PIDData turnPIDData = {30.0f, 0.01f, 1.0f}; |
sim642 | 27:24a9ac72fe92 | 13 | |
sim642 | 27:24a9ac72fe92 | 14 | /* |
sim642 | 27:24a9ac72fe92 | 15 | PWM timer channel |
sim642 | 27:24a9ac72fe92 | 16 | M1 1 2N |
sim642 | 27:24a9ac72fe92 | 17 | M2 4 2 |
sim642 | 27:24a9ac72fe92 | 18 | M3 2 1 |
sim642 | 27:24a9ac72fe92 | 19 | M4 4 4 |
sim642 | 27:24a9ac72fe92 | 20 | M5 1 3N |
sim642 | 27:24a9ac72fe92 | 21 | M6 2 2 |
sim642 | 27:24a9ac72fe92 | 22 | */ |
phairero | 26:c865244ca3cf | 23 | |
phairero | 26:c865244ca3cf | 24 | // 1 |
sim642 | 39:e35a99801ec1 | 25 | MotorData m1Data = {PB_0, PA_4, PC_0}; //PWM, Dir1, Dir2 |
sim642 | 39:e35a99801ec1 | 26 | EncoderData enc1Data = {PC_3, PC_1, 102.083 * 64}; //EncA, encB // https://www.pololu.com/product/2826 |
sim642 | 41:8ce8a3a47a92 | 27 | EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData); |
phairero | 32:9db688917ca3 | 28 | DigitalIn s1(PA_8); |
phairero | 26:c865244ca3cf | 29 | // 2 |
sim642 | 39:e35a99801ec1 | 30 | MotorData m2Data = {PB_7, PA_6, PB_9}; //PB7 = fault dir2 oli enne PC13 |
sim642 | 39:e35a99801ec1 | 31 | EncoderData enc2Data = {PC_14, PC_15, 102.083 * 64}; |
sim642 | 41:8ce8a3a47a92 | 32 | EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData); |
phairero | 32:9db688917ca3 | 33 | DigitalIn s2(PH_1); |
phairero | 26:c865244ca3cf | 34 | // 3 |
sim642 | 35:bce9ac485178 | 35 | MotorData m3Data = {PA_15, PC_12, PC_10}; |
sim642 | 35:bce9ac485178 | 36 | EncoderData enc3Data = {PC_11, PA_9, 102.083 * 64}; //B oli varem PA_13 |
sim642 | 41:8ce8a3a47a92 | 37 | EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData); |
phairero | 32:9db688917ca3 | 38 | DigitalIn s3(PA_14); |
phairero | 26:c865244ca3cf | 39 | // 4 |
phairero | 26:c865244ca3cf | 40 | MotorData m4Data = {PB_8, PC_6, PC_9}; |
phairero | 26:c865244ca3cf | 41 | EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64}; |
sim642 | 41:8ce8a3a47a92 | 42 | EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData); |
phairero | 32:9db688917ca3 | 43 | DigitalIn s4(PA_11); |
phairero | 26:c865244ca3cf | 44 | // 5 |
phairero | 26:c865244ca3cf | 45 | MotorData m5Data = {PB_15, PB_1, PB_2}; |
phairero | 33:ad2ebe22f8dd | 46 | EncoderData enc5Data = {PC_7, PB_6, 102.083 * 64}; //B oli varem PB_13, A oli varem PB_14 |
sim642 | 41:8ce8a3a47a92 | 47 | EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData); |
phairero | 32:9db688917ca3 | 48 | DigitalIn s5(PC_4); |
phairero | 26:c865244ca3cf | 49 | // 6 |
sim642 | 39:e35a99801ec1 | 50 | MotorData m6Data = {PB_3, PA_7, PB_5}; //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2 |
sim642 | 39:e35a99801ec1 | 51 | EncoderData enc6Data = {PB_4, PA_10, 102.083 * 64}; |
sim642 | 41:8ce8a3a47a92 | 52 | EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData); |
phairero | 32:9db688917ca3 | 53 | DigitalIn s6(PB_10); |
sim642 | 35:bce9ac485178 | 54 | |
phairero | 30:476ab400d765 | 55 | const int MOTORS = 6; |
phairero | 30:476ab400d765 | 56 | EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6}; |
phairero | 32:9db688917ca3 | 57 | DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6}; |
sim642 | 35:bce9ac485178 | 58 | |
phairero | 43:0627a2245b9d | 59 | float volatile speed; |
phairero | 43:0627a2245b9d | 60 | |
phairero | 43:0627a2245b9d | 61 | int volatile dir; |
phairero | 43:0627a2245b9d | 62 | |
phairero | 43:0627a2245b9d | 63 | int volatile turn; |
phairero | 43:0627a2245b9d | 64 | |
sim642 | 27:24a9ac72fe92 | 65 | Ticker ticker; |
sim642 | 27:24a9ac72fe92 | 66 | |
sim642 | 2:cf0147952fb9 | 67 | void rise() |
sim642 | 2:cf0147952fb9 | 68 | { |
sim642 | 24:fb1827be6f7e | 69 | //pc.printf("rise\n"); |
sim642 | 28:fd21faad6dd8 | 70 | //m1.drive(0); |
phairero | 36:906323851366 | 71 | //mod = mod*(-1); |
sim642 | 2:cf0147952fb9 | 72 | } |
phairero | 0:0b7259fdb68a | 73 | |
sim642 | 2:cf0147952fb9 | 74 | void fall() |
sim642 | 2:cf0147952fb9 | 75 | { |
sim642 | 24:fb1827be6f7e | 76 | //pc.printf("fall\n"); |
sim642 | 28:fd21faad6dd8 | 77 | //m1.drive(0.25); |
sim642 | 2:cf0147952fb9 | 78 | } |
phairero | 1:8b0322a353f4 | 79 | |
sim642 | 27:24a9ac72fe92 | 80 | void tick() |
sim642 | 27:24a9ac72fe92 | 81 | { |
sim642 | 28:fd21faad6dd8 | 82 | for (int i = 0; i < MOTORS; i++) |
sim642 | 28:fd21faad6dd8 | 83 | ms[i]->tick(); |
sim642 | 27:24a9ac72fe92 | 84 | } |
sim642 | 27:24a9ac72fe92 | 85 | |
sim642 | 27:24a9ac72fe92 | 86 | const float tickTime = 1.f / 60; |
sim642 | 27:24a9ac72fe92 | 87 | |
sim642 | 37:8021b3ce241a | 88 | void waitAllRotate() |
sim642 | 37:8021b3ce241a | 89 | { |
sim642 | 37:8021b3ce241a | 90 | tick(); |
sim642 | 37:8021b3ce241a | 91 | |
sim642 | 37:8021b3ce241a | 92 | float sum; |
sim642 | 37:8021b3ce241a | 93 | do { |
sim642 | 37:8021b3ce241a | 94 | sum = 0; |
sim642 | 37:8021b3ce241a | 95 | for (int i = 0; i < MOTORS; i++) |
sim642 | 37:8021b3ce241a | 96 | sum += abs(ms[i]->errorTurn); |
phairero | 43:0627a2245b9d | 97 | pc.printf(""); |
phairero | 43:0627a2245b9d | 98 | //pc.printf("%f\n",sum); |
sim642 | 37:8021b3ce241a | 99 | } |
sim642 | 37:8021b3ce241a | 100 | while(sum > 0.05f); |
sim642 | 37:8021b3ce241a | 101 | |
sim642 | 37:8021b3ce241a | 102 | wait(0.1f); |
sim642 | 37:8021b3ce241a | 103 | } |
sim642 | 37:8021b3ce241a | 104 | |
sim642 | 42:7fa713d5d1af | 105 | void calibrateLegs() |
sim642 | 42:7fa713d5d1af | 106 | { |
sim642 | 37:8021b3ce241a | 107 | for (int i = 0; i < MOTORS; i++) |
sim642 | 37:8021b3ce241a | 108 | { |
sim642 | 40:01a97bc4ef7a | 109 | if (i != 3) |
sim642 | 39:e35a99801ec1 | 110 | ms[i]->drive(0.2f); |
sim642 | 40:01a97bc4ef7a | 111 | } |
sim642 | 40:01a97bc4ef7a | 112 | |
sim642 | 40:01a97bc4ef7a | 113 | bool done; |
sim642 | 40:01a97bc4ef7a | 114 | do |
sim642 | 40:01a97bc4ef7a | 115 | { |
sim642 | 40:01a97bc4ef7a | 116 | done = true; |
sim642 | 40:01a97bc4ef7a | 117 | for (int i = 0; i < MOTORS; i++) |
sim642 | 40:01a97bc4ef7a | 118 | { |
sim642 | 40:01a97bc4ef7a | 119 | if (i != 3) |
sim642 | 40:01a97bc4ef7a | 120 | { |
sim642 | 40:01a97bc4ef7a | 121 | if (ss[i].read()) |
sim642 | 40:01a97bc4ef7a | 122 | done = false; |
sim642 | 40:01a97bc4ef7a | 123 | else |
sim642 | 40:01a97bc4ef7a | 124 | ms[i]->drive(0.f); |
sim642 | 40:01a97bc4ef7a | 125 | } |
sim642 | 37:8021b3ce241a | 126 | } |
sim642 | 40:01a97bc4ef7a | 127 | } |
sim642 | 40:01a97bc4ef7a | 128 | while (!done); |
sim642 | 40:01a97bc4ef7a | 129 | |
sim642 | 40:01a97bc4ef7a | 130 | for (int i = 0; i < MOTORS; i++) |
sim642 | 37:8021b3ce241a | 131 | ms[i]->getEncoder().reset(); |
sim642 | 42:7fa713d5d1af | 132 | |
phairero | 43:0627a2245b9d | 133 | pc.printf("done\n"); |
sim642 | 42:7fa713d5d1af | 134 | } |
sim642 | 42:7fa713d5d1af | 135 | |
sim642 | 42:7fa713d5d1af | 136 | void standUp() |
sim642 | 42:7fa713d5d1af | 137 | { |
phairero | 44:c2acf8d5e191 | 138 | ms[0]->rotate(0.5f + 0.0625f, 0.75f); |
phairero | 44:c2acf8d5e191 | 139 | ms[1]->rotate(0.5f - 0.0625f, 0.75f); |
phairero | 44:c2acf8d5e191 | 140 | ms[2]->rotate(0.5f + 0.0625f, 0.75f); |
phairero | 44:c2acf8d5e191 | 141 | ms[3]->rotate(0.5f - 0.0625f, 0.75f); |
phairero | 44:c2acf8d5e191 | 142 | ms[4]->rotate(0.5f + 0.0625f, 0.75f); |
phairero | 44:c2acf8d5e191 | 143 | ms[5]->rotate(0.5f - 0.0625f, 0.75f); |
sim642 | 37:8021b3ce241a | 144 | waitAllRotate(); |
sim642 | 42:7fa713d5d1af | 145 | } |
sim642 | 42:7fa713d5d1af | 146 | |
phairero | 43:0627a2245b9d | 147 | void moveOne() |
phairero | 43:0627a2245b9d | 148 | { |
phairero | 44:c2acf8d5e191 | 149 | |
phairero | 44:c2acf8d5e191 | 150 | ms[0]->rotate(dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 151 | ms[1]->rotate(dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 152 | ms[2]->rotate(dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 153 | ms[3]->rotate(dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 154 | ms[4]->rotate(dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 155 | ms[5]->rotate(dir*0.125f,speed/3); |
phairero | 43:0627a2245b9d | 156 | waitAllRotate(); |
phairero | 43:0627a2245b9d | 157 | } |
phairero | 43:0627a2245b9d | 158 | |
phairero | 43:0627a2245b9d | 159 | void moveTwo(){ |
phairero | 44:c2acf8d5e191 | 160 | ms[0]->rotate(dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 161 | ms[1]->rotate(dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 162 | ms[2]->rotate(dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 163 | ms[3]->rotate(dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 164 | ms[4]->rotate(dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 165 | ms[5]->rotate(dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 166 | waitAllRotate(); |
phairero | 44:c2acf8d5e191 | 167 | } |
phairero | 44:c2acf8d5e191 | 168 | |
phairero | 44:c2acf8d5e191 | 169 | void turnOne(){ |
phairero | 44:c2acf8d5e191 | 170 | ms[0]->rotate(turn*dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 171 | ms[1]->rotate(turn*dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 172 | ms[2]->rotate(-turn*dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 173 | ms[3]->rotate(-turn*dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 174 | ms[4]->rotate(-turn*dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 175 | ms[5]->rotate(turn*dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 176 | waitAllRotate(); |
phairero | 44:c2acf8d5e191 | 177 | } |
phairero | 44:c2acf8d5e191 | 178 | |
phairero | 44:c2acf8d5e191 | 179 | void turnTwo(){ |
phairero | 44:c2acf8d5e191 | 180 | ms[0]->rotate(turn*dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 181 | ms[1]->drive(0.0); |
phairero | 44:c2acf8d5e191 | 182 | ms[2]->rotate(-turn*dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 183 | ms[3]->drive(0.0); |
phairero | 44:c2acf8d5e191 | 184 | ms[4]->rotate(-turn*dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 185 | ms[5]->drive(0.0); |
phairero | 44:c2acf8d5e191 | 186 | waitAllRotate(); |
phairero | 44:c2acf8d5e191 | 187 | } |
phairero | 44:c2acf8d5e191 | 188 | |
phairero | 44:c2acf8d5e191 | 189 | void turnThree(){ |
phairero | 44:c2acf8d5e191 | 190 | ms[0]->drive(0.0); |
phairero | 44:c2acf8d5e191 | 191 | ms[1]->rotate(turn*dir*0.875f,speed); |
phairero | 44:c2acf8d5e191 | 192 | ms[2]->drive(0.0); |
phairero | 44:c2acf8d5e191 | 193 | ms[3]->rotate(-turn*dir*0.125f,speed/3); |
phairero | 44:c2acf8d5e191 | 194 | ms[4]->drive(0.0); |
phairero | 44:c2acf8d5e191 | 195 | ms[5]->rotate(turn*dir*0.875f,speed); |
phairero | 43:0627a2245b9d | 196 | waitAllRotate(); |
phairero | 43:0627a2245b9d | 197 | } |
phairero | 43:0627a2245b9d | 198 | |
phairero | 43:0627a2245b9d | 199 | |
sim642 | 42:7fa713d5d1af | 200 | int main() |
sim642 | 42:7fa713d5d1af | 201 | { |
sim642 | 42:7fa713d5d1af | 202 | pc.printf("MAIN\n"); |
sim642 | 42:7fa713d5d1af | 203 | bt.rise(&rise); |
sim642 | 42:7fa713d5d1af | 204 | bt.fall(&fall); |
sim642 | 42:7fa713d5d1af | 205 | |
sim642 | 42:7fa713d5d1af | 206 | //ms[active]->drive(0.25); |
sim642 | 42:7fa713d5d1af | 207 | |
sim642 | 42:7fa713d5d1af | 208 | ticker.attach(&tick, tickTime); |
sim642 | 42:7fa713d5d1af | 209 | |
sim642 | 42:7fa713d5d1af | 210 | //unsigned char volatile rfget; |
sim642 | 42:7fa713d5d1af | 211 | |
sim642 | 42:7fa713d5d1af | 212 | calibrateLegs(); |
sim642 | 42:7fa713d5d1af | 213 | standUp(); |
sim642 | 37:8021b3ce241a | 214 | |
phairero | 43:0627a2245b9d | 215 | //float volatile sum; |
phairero | 36:906323851366 | 216 | |
phairero | 43:0627a2245b9d | 217 | speed = 0.0f; |
phairero | 43:0627a2245b9d | 218 | dir = 1; |
phairero | 43:0627a2245b9d | 219 | turn = 0; |
phairero | 36:906323851366 | 220 | |
phairero | 43:0627a2245b9d | 221 | pc.printf("start\n"); |
sim642 | 2:cf0147952fb9 | 222 | while(1) |
phairero | 36:906323851366 | 223 | { |
phairero | 36:906323851366 | 224 | |
phairero | 43:0627a2245b9d | 225 | //speed = 1.5f; |
sim642 | 37:8021b3ce241a | 226 | //speed = 0; |
phairero | 36:906323851366 | 227 | //int oldactive = active; |
phairero | 43:0627a2245b9d | 228 | if (pc.readable()){ |
phairero | 43:0627a2245b9d | 229 | pc.scanf("%f %d %d", &speed, &dir, &turn); |
phairero | 44:c2acf8d5e191 | 230 | if (speed >= 10) |
phairero | 43:0627a2245b9d | 231 | speed = speed - (((int)(speed/10))*10); |
phairero | 44:c2acf8d5e191 | 232 | else if (speed <= 0.25f) |
phairero | 44:c2acf8d5e191 | 233 | speed = float(0); |
phairero | 44:c2acf8d5e191 | 234 | } |
phairero | 43:0627a2245b9d | 235 | pc.printf("%f %d%d\n",speed,dir,turn); |
phairero | 43:0627a2245b9d | 236 | |
phairero | 36:906323851366 | 237 | if (speed != 0){ |
phairero | 43:0627a2245b9d | 238 | //for (int i = 0; i < MOTORS; i++) |
phairero | 43:0627a2245b9d | 239 | // pc.printf("%ld ", ms[i]->encoder.getCount()); |
phairero | 43:0627a2245b9d | 240 | //pc.printf("\n"); |
phairero | 43:0627a2245b9d | 241 | |
phairero | 43:0627a2245b9d | 242 | //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod; |
phairero | 44:c2acf8d5e191 | 243 | if (turn == 0) { |
phairero | 44:c2acf8d5e191 | 244 | if (dir == 1){ |
phairero | 44:c2acf8d5e191 | 245 | moveOne(); |
phairero | 44:c2acf8d5e191 | 246 | moveTwo(); |
phairero | 44:c2acf8d5e191 | 247 | } |
phairero | 44:c2acf8d5e191 | 248 | else { |
phairero | 44:c2acf8d5e191 | 249 | moveTwo(); |
phairero | 44:c2acf8d5e191 | 250 | moveOne(); |
phairero | 43:0627a2245b9d | 251 | } |
phairero | 44:c2acf8d5e191 | 252 | } |
phairero | 43:0627a2245b9d | 253 | else { |
phairero | 44:c2acf8d5e191 | 254 | if ((turn*dir) == 1){ |
phairero | 44:c2acf8d5e191 | 255 | turnOne(); |
phairero | 44:c2acf8d5e191 | 256 | turnTwo(); |
phairero | 44:c2acf8d5e191 | 257 | turnThree(); |
phairero | 43:0627a2245b9d | 258 | } |
phairero | 44:c2acf8d5e191 | 259 | else { |
phairero | 44:c2acf8d5e191 | 260 | turnThree(); |
phairero | 44:c2acf8d5e191 | 261 | turnTwo(); |
phairero | 44:c2acf8d5e191 | 262 | turnOne(); |
phairero | 44:c2acf8d5e191 | 263 | } |
phairero | 44:c2acf8d5e191 | 264 | } |
phairero | 43:0627a2245b9d | 265 | |
phairero | 43:0627a2245b9d | 266 | /*ms[0]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 267 | ms[1]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 268 | ms[2]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 269 | ms[3]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 270 | ms[4]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 271 | ms[5]->rotate(dir*0.25f,speed/3); |
sim642 | 37:8021b3ce241a | 272 | waitAllRotate(); |
sim642 | 37:8021b3ce241a | 273 | |
phairero | 43:0627a2245b9d | 274 | ms[0]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 275 | ms[1]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 276 | ms[2]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 277 | ms[3]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 278 | ms[4]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 279 | ms[5]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 280 | waitAllRotate();*/ |
phairero | 36:906323851366 | 281 | } |
phairero | 36:906323851366 | 282 | else { |
phairero | 44:c2acf8d5e191 | 283 | ms[0]->rotate(0.0f, 0.0f); |
phairero | 44:c2acf8d5e191 | 284 | ms[1]->rotate(0.0f, 0.0f); |
phairero | 44:c2acf8d5e191 | 285 | ms[2]->rotate(0.0f, 0.0f); |
phairero | 44:c2acf8d5e191 | 286 | ms[3]->rotate(0.0f, 0.0f); |
phairero | 44:c2acf8d5e191 | 287 | ms[4]->rotate(0.0f, 0.0f); |
phairero | 44:c2acf8d5e191 | 288 | ms[5]->rotate(0.0f, 0.0f); |
phairero | 36:906323851366 | 289 | } |
phairero | 36:906323851366 | 290 | |
phairero | 36:906323851366 | 291 | //pc.printf("%f", speed); |
phairero | 32:9db688917ca3 | 292 | //info enkoodritelt |
phairero | 36:906323851366 | 293 | /* |
sim642 | 29:f905e66e1a6f | 294 | for (int i = 0; i < MOTORS; i++) |
sim642 | 29:f905e66e1a6f | 295 | pc.printf("%ld ", ms[i]->encoder.getCount()); |
sim642 | 29:f905e66e1a6f | 296 | pc.printf("\n"); |
phairero | 36:906323851366 | 297 | */ |
sim642 | 37:8021b3ce241a | 298 | |
phairero | 32:9db688917ca3 | 299 | //info IRidelt |
sim642 | 37:8021b3ce241a | 300 | /*for (int i = 0; i < MOTORS; i++) |
phairero | 32:9db688917ca3 | 301 | pc.printf("%d ", ss[i].read()); |
phairero | 32:9db688917ca3 | 302 | pc.printf("\n"); |
sim642 | 37:8021b3ce241a | 303 | wait(1.0f);*/ |
sim642 | 37:8021b3ce241a | 304 | |
sim642 | 28:fd21faad6dd8 | 305 | //pc.printf("%d", active); |
phairero | 31:8d8d31b6d8cd | 306 | //pc.scanf("%f", &turn); |
sim642 | 23:d844cc906b66 | 307 | //m.rotate(turn, 0.2); |
phairero | 31:8d8d31b6d8cd | 308 | //pc.scanf("%f %f", &rot, &speed); |
sim642 | 28:fd21faad6dd8 | 309 | //m1.rotate(rot, speed); |
sim642 | 23:d844cc906b66 | 310 | //m.drive(speed); |
phairero | 31:8d8d31b6d8cd | 311 | //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed()); |
phairero | 31:8d8d31b6d8cd | 312 | //pc.printf("%f %f\n", m.s, m.getSetSpeed()); |
phairero | 31:8d8d31b6d8cd | 313 | //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn()); |
phairero | 31:8d8d31b6d8cd | 314 | //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn()); |
phairero | 36:906323851366 | 315 | |
phairero | 0:0b7259fdb68a | 316 | } |
phairero | 26:c865244ca3cf | 317 | } |