Rauno U
/
Miisu
Six crescent shaped legs
Diff: main.cpp
- Revision:
- 40:01a97bc4ef7a
- Parent:
- 39:e35a99801ec1
- Child:
- 41:8ce8a3a47a92
diff -r e35a99801ec1 -r 01a97bc4ef7a main.cpp --- a/main.cpp Wed Jun 15 14:50:08 2016 +0000 +++ b/main.cpp Wed Jun 15 15:09:37 2016 +0000 @@ -112,15 +112,29 @@ for (int i = 0; i < MOTORS; i++) { - if (!(i == 2 || i == 3)) - { - pc.printf("calib %d\n", i); + if (i != 3) ms[i]->drive(0.2f); - while (ss[i].read()); + } + + bool done; + do + { + done = true; + for (int i = 0; i < MOTORS; i++) + { + if (i != 3) + { + if (ss[i].read()) + done = false; + else + ms[i]->drive(0.f); + } } - ms[i]->drive(0.f); + } + while (!done); + + for (int i = 0; i < MOTORS; i++) ms[i]->getEncoder().reset(); - } pc.printf("done"); for (int i = 0; i < MOTORS; i++)