Rauno U
/
Miisu
Six crescent shaped legs
Diff: main.cpp
- Revision:
- 37:8021b3ce241a
- Parent:
- 36:906323851366
- Child:
- 38:b03a5bf9ac7b
diff -r 906323851366 -r 8021b3ce241a main.cpp --- a/main.cpp Wed Jun 15 11:52:08 2016 +0000 +++ b/main.cpp Wed Jun 15 14:09:49 2016 +0000 @@ -82,6 +82,22 @@ const float tickTime = 1.f / 60; +void waitAllRotate() +{ + tick(); + + float sum; + do { + sum = 0; + for (int i = 0; i < MOTORS; i++) + sum += abs(ms[i]->errorTurn); + pc.printf("%f\n",sum); + } + while(sum > 0.05f); + + wait(0.1f); +} + int main() { pc.printf("MAIN\n"); @@ -94,6 +110,38 @@ //unsigned char volatile rfget; + for (int i = 0; i < MOTORS; i++) + { + if (!(i == 2 || i == 3)) + { + pc.printf("calib %d\n", i); + ms[i]->drive(i == 0 || i == 1 || i == 5 ? -0.3f : 0.3f); + while (ss[i].read()); + } + ms[i]->drive(0.f); + ms[i]->getEncoder().reset(); + } + pc.printf("done"); + + for (int i = 0; i < MOTORS; i++) + pc.printf("%ld ", ms[i]->encoder.getCount()); + pc.printf("\n"); + + /*ms[0]->rotate(-0.5f + 0.125f, 0.5f); + ms[1]->rotate(-0.5f - 0.125f, 0.5f); + ms[2]->rotate(0.5f - 0.125f, 0.5f); + ms[3]->rotate(0.5f + 0.125f, 0.5f); + ms[4]->rotate(0.5f - 0.125f, 0.5f); + ms[5]->rotate(-0.5f - 0.125f, 0.5f); + waitAllRotate();*/ + ms[0]->rotate(-0.5f - 0.125f, 0.5f); + ms[1]->rotate(-0.5f + 0.125f, 0.5f); + ms[2]->rotate(0.5f + 0.125f, 0.5f); + ms[3]->rotate(0.5f - 0.125f, 0.5f); + ms[4]->rotate(0.5f + 0.125f, 0.5f); + ms[5]->rotate(-0.5f + 0.125f, 0.5f); + waitAllRotate(); + float volatile speed; float volatile sum; @@ -103,10 +151,15 @@ { speed = 0.925f; + //speed = 0; //int oldactive = active; //pc.scanf("%f", &speed); if (speed != 0){ + for (int i = 0; i < MOTORS; i++) + pc.printf("%ld ", ms[i]->encoder.getCount()); + pc.printf("\n"); + speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod; ms[0]->rotate(-0.75f,speed*3); @@ -115,30 +168,15 @@ ms[3]->rotate(0.25f,speed); ms[4]->rotate(0.75f,speed*3); ms[5]->rotate(-0.25f,speed); - tick(); - do { - sum = 0; - for (int i = 0; i < MOTORS; i++){ - pc.printf("%d,%f\n",i,ms[i]->errorTurn); - sum+=abs(ms[i]->errorTurn); - } - pc.printf("1)%f\n",sum); - } while( sum > 0.05f ); + waitAllRotate(); + ms[0]->rotate(-0.25f,speed); ms[1]->rotate(-0.75f,speed*3); ms[2]->rotate(0.25f,speed); ms[3]->rotate(0.75f,speed*3); ms[4]->rotate(0.25f,speed); ms[5]->rotate(-0.75f,speed*3); - tick(); - do { - sum = 0; - for (int i = 0; i < MOTORS; i++){ - sum+=abs(ms[i]->errorTurn); - } - //pc.printf("2)%f\n",sum); - } while( sum > 0.05f ); - wait(0.1f); + waitAllRotate(); } else { ms[0]->drive(0.0); @@ -156,13 +194,13 @@ pc.printf("%ld ", ms[i]->encoder.getCount()); pc.printf("\n"); */ - /* + //info IRidelt - for (int i = 0; i < MOTORS; i++) + /*for (int i = 0; i < MOTORS; i++) pc.printf("%d ", ss[i].read()); pc.printf("\n"); - wait(1.0f); - */ + wait(1.0f);*/ + //pc.printf("%d", active); //pc.scanf("%f", &turn); //m.rotate(turn, 0.2);