Rauno U
/
Miisu
Six crescent shaped legs
Diff: main.cpp
- Revision:
- 43:0627a2245b9d
- Parent:
- 42:7fa713d5d1af
- Child:
- 44:c2acf8d5e191
diff -r 7fa713d5d1af -r 0627a2245b9d main.cpp --- a/main.cpp Wed Jun 15 18:52:43 2016 +0000 +++ b/main.cpp Thu Jun 16 16:56:25 2016 +0000 @@ -54,9 +54,14 @@ const int MOTORS = 6; EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6}; -volatile float mod = 0.25; DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6}; +float volatile speed; + +int volatile dir; + +int volatile turn; + Ticker ticker; void rise() @@ -89,7 +94,8 @@ sum = 0; for (int i = 0; i < MOTORS; i++) sum += abs(ms[i]->errorTurn); - pc.printf("%f\n",sum); + pc.printf(""); + //pc.printf("%f\n",sum); } while(sum > 0.05f); @@ -124,7 +130,7 @@ for (int i = 0; i < MOTORS; i++) ms[i]->getEncoder().reset(); - pc.printf("done"); + pc.printf("done\n"); } void standUp() @@ -138,6 +144,28 @@ waitAllRotate(); } +void moveOne() +{ + ms[0]->rotate(dir*0.75f,speed); + ms[1]->rotate(dir*0.25f,speed/3); + ms[2]->rotate(dir*0.75f,speed); + ms[3]->rotate(dir*0.25f,speed/3); + ms[4]->rotate(dir*0.75f,speed); + ms[5]->rotate(dir*0.25f,speed/3); + waitAllRotate(); +} + +void moveTwo(){ + ms[0]->rotate(dir*0.25f,speed/3); + ms[1]->rotate(dir*0.75f,speed); + ms[2]->rotate(dir*0.25f,speed/3); + ms[3]->rotate(dir*0.75f,speed); + ms[4]->rotate(dir*0.25f,speed/3); + ms[5]->rotate(dir*0.75f,speed); + waitAllRotate(); +} + + int main() { pc.printf("MAIN\n"); @@ -153,40 +181,58 @@ calibrateLegs(); standUp(); - float volatile speed; + //float volatile sum; - float volatile sum; + speed = 0.0f; + dir = 1; + turn = 0; + pc.printf("start\n"); while(1) { - speed = 1.5f; + //speed = 1.5f; //speed = 0; //int oldactive = active; - //pc.scanf("%f", &speed); - + if (pc.readable()){ + pc.scanf("%f %d %d", &speed, &dir, &turn); + if (speed >= 10){ + speed = speed - (((int)(speed/10))*10); + } + } + pc.printf("%f %d%d\n",speed,dir,turn); + if (speed != 0){ - for (int i = 0; i < MOTORS; i++) - pc.printf("%ld ", ms[i]->encoder.getCount()); - pc.printf("\n"); + //for (int i = 0; i < MOTORS; i++) + // pc.printf("%ld ", ms[i]->encoder.getCount()); + //pc.printf("\n"); + + //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod; - speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod; - - ms[0]->rotate(0.75f,speed*3); - ms[1]->rotate(0.25f,speed); - ms[2]->rotate(0.75f,speed*3); - ms[3]->rotate(0.25f,speed); - ms[4]->rotate(0.75f,speed*3); - ms[5]->rotate(0.25f,speed); + if (dir == 1){ + moveOne(); + moveTwo(); + } + else { + moveTwo(); + moveOne(); + } + + /*ms[0]->rotate(dir*0.75f,speed); + ms[1]->rotate(dir*0.25f,speed/3); + ms[2]->rotate(dir*0.75f,speed); + ms[3]->rotate(dir*0.25f,speed/3); + ms[4]->rotate(dir*0.75f,speed); + ms[5]->rotate(dir*0.25f,speed/3); 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); - waitAllRotate(); + ms[0]->rotate(dir*0.25f,speed/3); + ms[1]->rotate(dir*0.75f,speed); + ms[2]->rotate(dir*0.25f,speed/3); + ms[3]->rotate(dir*0.75f,speed); + ms[4]->rotate(dir*0.25f,speed/3); + ms[5]->rotate(dir*0.75f,speed); + waitAllRotate();*/ } else { ms[0]->drive(0.0);