Rauno U
/
Miisu
Six crescent shaped legs
Revision 47:4f418a4b0051, committed 2016-06-21
- Comitter:
- sim642
- Date:
- Tue Jun 21 14:43:44 2016 +0000
- Parent:
- 46:49f3da891e24
- Commit message:
- Byte based communication
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 49f3da891e24 -r 4f418a4b0051 main.cpp --- a/main.cpp Tue Jun 21 11:41:18 2016 +0000 +++ b/main.cpp Tue Jun 21 14:43:44 2016 +0000 @@ -56,10 +56,12 @@ EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6}; DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6}; +float volatile rxSpeed; +int volatile rxDir; +int volatile rxTurn; + float volatile speed; - int volatile dir; - int volatile turn; Ticker ticker; @@ -95,8 +97,8 @@ for (int i = 0; i < MOTORS; i++) sum += abs(ms[i]->errorTurn); - pc.printf(""); - //pc.printf("%f\n",sum); + //pc.printf(""); + pc.printf("%f\n",sum); } while(sum > 0.05f); @@ -107,7 +109,7 @@ { for (int i = 0; i < MOTORS; i++) { - ms[i]->drive(0.2f); + ms[i]->drive(0.60f); } bool done; @@ -141,40 +143,42 @@ waitAllRotate(); } +float groundDivide = 4; + void moveOne() { ms[0]->rotate(dir*0.875f,speed); - ms[1]->rotate(dir*0.125f,speed/3); + ms[1]->rotate(dir*0.125f,speed/groundDivide); ms[2]->rotate(dir*0.875f,speed); - ms[3]->rotate(dir*0.125f,speed/3); + ms[3]->rotate(dir*0.125f,speed/groundDivide); ms[4]->rotate(dir*0.875f,speed); - ms[5]->rotate(dir*0.125f,speed/3); + ms[5]->rotate(dir*0.125f,speed/groundDivide); waitAllRotate(); } void moveTwo(){ - ms[0]->rotate(dir*0.125f,speed/3); + ms[0]->rotate(dir*0.125f,speed/groundDivide); ms[1]->rotate(dir*0.875f,speed); - ms[2]->rotate(dir*0.125f,speed/3); + ms[2]->rotate(dir*0.125f,speed/groundDivide); ms[3]->rotate(dir*0.875f,speed); - ms[4]->rotate(dir*0.125f,speed/3); + ms[4]->rotate(dir*0.125f,speed/groundDivide); ms[5]->rotate(dir*0.875f,speed); waitAllRotate(); } void turnOne(){ ms[0]->rotate(turn*dir*0.875f,speed); - ms[1]->rotate(turn*dir*0.125f,speed/3); - ms[2]->rotate(-turn*dir*0.125f,speed/3); + ms[1]->rotate(turn*dir*0.125f,speed/groundDivide); + ms[2]->rotate(-turn*dir*0.125f,speed/groundDivide); ms[3]->rotate(-turn*dir*0.875f,speed); - ms[4]->rotate(-turn*dir*0.125f,speed/3); - ms[5]->rotate(turn*dir*0.125f,speed/3); + ms[4]->rotate(-turn*dir*0.125f,speed/groundDivide); + ms[5]->rotate(turn*dir*0.125f,speed/groundDivide); waitAllRotate(); } void turnTwo(){ - ms[0]->rotate(turn*dir*0.125f,speed/3); + ms[0]->rotate(turn*dir*0.125f,speed/groundDivide); ms[1]->rotate(0.0f, 1.0f); ms[2]->rotate(-turn*dir*0.875f,speed); ms[3]->rotate(0.0f, 1.0f); @@ -187,15 +191,47 @@ ms[0]->rotate(0.0f, 1.0f); ms[1]->rotate(turn*dir*0.875f,speed); ms[2]->rotate(0.0f, 1.0f); - ms[3]->rotate(-turn*dir*0.125f,speed/3); + ms[3]->rotate(-turn*dir*0.125f,speed/groundDivide); ms[4]->rotate(0.0f, 1.0f); ms[5]->rotate(turn*dir*0.875f,speed); waitAllRotate(); } +void rfRx() +{ + int b = pc.getc(); + int rfSpeed = b & 0x3F; + + rxSpeed = rfSpeed / 10.f * 2; + + switch ((b >> 6) & 0x3) + { + case 0: // fwd + rxDir = 1; + rxTurn = 0; + break; + + case 1: // bwd + rxDir = -1; + rxTurn = 0; + break; + + case 2: // left + rxDir = 1; + rxTurn = -1; + break; + + case 3: // right + rxDir = 1; + rxTurn = 1; + break; + } +} int main() -{ +{ + pc.attach(&rfRx); + pc.printf("MAIN\n"); bt.rise(&rise); bt.fall(&fall); @@ -211,27 +247,18 @@ //float volatile sum; - speed = 0.0f; - dir = 1; - turn = 0; + rxSpeed = 0.0f; + rxDir = 1; + rxTurn = 0; pc.printf("start\n"); while(1) { - - //speed = 1.5f; - //speed = 0; - //int oldactive = active; - //if (pc.readable()){ - int rfSpeed = 0; - pc.scanf("%d %d %d", &rfSpeed, &dir, &turn); - if (rfSpeed > 50){ - rfSpeed = rfSpeed-(rfSpeed - rfSpeed/10); - } - speed = rfSpeed/10.0f; - if (speed <= 0.5f) - speed = float(0); - pc.printf("%d %.2f\n",rfSpeed,speed); + speed = rxSpeed; + dir = rxDir; + turn = rxTurn; + + //pc.printf("%.2f %d %d\n", speed, dir, turn); if (speed != 0){ //for (int i = 0; i < MOTORS; i++)