Ryu Kaz
/
767zi
t6est
foot.cpp@4:9ba47e5db1e2, 2019-10-05 (annotated)
- Committer:
- kazuryu
- Date:
- Sat Oct 05 01:41:37 2019 +0000
- Revision:
- 4:9ba47e5db1e2
- Parent:
- 0:3dc012104243
B
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazuryu | 0:3dc012104243 | 1 | #include "mbed.h" |
kazuryu | 0:3dc012104243 | 2 | #include "foot.h" |
kazuryu | 0:3dc012104243 | 3 | /* |
kazuryu | 0:3dc012104243 | 4 | PwmOut RF(D3); |
kazuryu | 0:3dc012104243 | 5 | PwmOut LF(D5); |
kazuryu | 0:3dc012104243 | 6 | PwmOut RB(D6); |
kazuryu | 0:3dc012104243 | 7 | PwmOut LB(D9); |
kazuryu | 0:3dc012104243 | 8 | PwmOut pwms[4] = {RF,LF,RB,LB}; |
kazuryu | 0:3dc012104243 | 9 | */ |
kazuryu | 0:3dc012104243 | 10 | // |
kazuryu | 0:3dc012104243 | 11 | DigitalOut dig(PG_9); |
kazuryu | 0:3dc012104243 | 12 | PwmOut RFA(PB_4);//3/1 |
kazuryu | 0:3dc012104243 | 13 | PwmOut RFB(PB_5);//3/2 |
kazuryu | 0:3dc012104243 | 14 | PwmOut LFA(PC_8);//3/3 |
kazuryu | 0:3dc012104243 | 15 | PwmOut LFB(PC_9);//3/4 |
kazuryu | 0:3dc012104243 | 16 | PwmOut RBA(PD_12);//4/1 |
kazuryu | 0:3dc012104243 | 17 | PwmOut RBB(PD_13);//4/2 |
kazuryu | 0:3dc012104243 | 18 | PwmOut LBA(PD_14);//4/3 |
kazuryu | 0:3dc012104243 | 19 | PwmOut LBB(PD_15);//4/4 |
kazuryu | 0:3dc012104243 | 20 | PwmOut pwms[8] = {RFA,RFB,LFA,LFB,RBA,RBB,LBA,LBB}; |
kazuryu | 0:3dc012104243 | 21 | PwmOut lockR(PB_10); |
kazuryu | 0:3dc012104243 | 22 | PwmOut lockL(PB_11); |
kazuryu | 4:9ba47e5db1e2 | 23 | |
kazuryu | 0:3dc012104243 | 24 | //new |
kazuryu | 0:3dc012104243 | 25 | //PB_6 -> PD_12 |
kazuryu | 0:3dc012104243 | 26 | //PC_7 -> PB_5 |
kazuryu | 0:3dc012104243 | 27 | |
kazuryu | 0:3dc012104243 | 28 | /* |
kazuryu | 0:3dc012104243 | 29 | // PB_4 -> PA_6///// |
kazuryu | 0:3dc012104243 | 30 | // PB_6 -> PD_12 |
kazuryu | 0:3dc012104243 | 31 | */ |
kazuryu | 0:3dc012104243 | 32 | /* |
kazuryu | 0:3dc012104243 | 33 | DigitalOut dig(PG_9); |
kazuryu | 0:3dc012104243 | 34 | PwmOut RFA(PB_4);//3/1 |
kazuryu | 0:3dc012104243 | 35 | PwmOut RFB(PC_7);//3/2 |
kazuryu | 0:3dc012104243 | 36 | PwmOut LFA(PC_8);//3/3 |
kazuryu | 0:3dc012104243 | 37 | PwmOut LFB(PC_9);//3/4 |
kazuryu | 0:3dc012104243 | 38 | PwmOut RBA(PB_6);//4/1 |
kazuryu | 0:3dc012104243 | 39 | PwmOut RBB(PD_13);//4/2 |
kazuryu | 0:3dc012104243 | 40 | PwmOut LBA(PD_14);//4/3 |
kazuryu | 0:3dc012104243 | 41 | PwmOut LBB(PD_15);//4/4 |
kazuryu | 0:3dc012104243 | 42 | */ |
kazuryu | 0:3dc012104243 | 43 | int Write[][4] { |
kazuryu | 0:3dc012104243 | 44 | {255, 255, 255, 255}, //0前 |
kazuryu | 0:3dc012104243 | 45 | { -255, -255, -255, -255}, //1後ろ |
kazuryu | 0:3dc012104243 | 46 | { -255, 255, 255, -255}, //2右 |
kazuryu | 0:3dc012104243 | 47 | {255, -255, -255, 255}, //3左 |
kazuryu | 0:3dc012104243 | 48 | {0, 255, 255, 0}, //4右前 |
kazuryu | 0:3dc012104243 | 49 | {255, 0, 0, 255}, //5左前 |
kazuryu | 0:3dc012104243 | 50 | { -255, 0, 0, -255}, //6右後ろ |
kazuryu | 0:3dc012104243 | 51 | {0, -255, -255, 0}, //7左後ろ |
kazuryu | 0:3dc012104243 | 52 | { -255, 255, -255, 255}, //8右旋回 |
kazuryu | 0:3dc012104243 | 53 | {255, -255, 255, -255},//9左旋回 |
kazuryu | 0:3dc012104243 | 54 | {500,500,500,500} |
kazuryu | 0:3dc012104243 | 55 | }; |
kazuryu | 4:9ba47e5db1e2 | 56 | //1.1052f,1,1.0421f,1.0726f |
kazuryu | 4:9ba47e5db1e2 | 57 | float pwm_g[] = {1,0.904f,0.982f,0.970f}; |
kazuryu | 4:9ba47e5db1e2 | 58 | //1,0.904,0.942,0.970 |
kazuryu | 0:3dc012104243 | 59 | void Move(int way,float pwm){ |
kazuryu | 0:3dc012104243 | 60 | int num; |
kazuryu | 0:3dc012104243 | 61 | for(int i =0;i<4;i++){ |
kazuryu | 0:3dc012104243 | 62 | num = Write[way][i]; |
kazuryu | 0:3dc012104243 | 63 | if(num > 0)dig = 0; |
kazuryu | 0:3dc012104243 | 64 | else if(num < 0)dig = 1; |
kazuryu | 0:3dc012104243 | 65 | if(num != 0)pwms[i] = pwm; |
kazuryu | 0:3dc012104243 | 66 | else pwms[i] = 0; |
kazuryu | 0:3dc012104243 | 67 | } |
kazuryu | 0:3dc012104243 | 68 | } |
kazuryu | 0:3dc012104243 | 69 | int count_Move = 0; |
kazuryu | 0:3dc012104243 | 70 | int count_Move_lock = 0; |
kazuryu | 0:3dc012104243 | 71 | void Move2(int way,float pwm){ |
kazuryu | 0:3dc012104243 | 72 | int num; |
kazuryu | 0:3dc012104243 | 73 | if(count_Move == 0){ |
kazuryu | 0:3dc012104243 | 74 | for(int i =0;i<8;i++){ |
kazuryu | 0:3dc012104243 | 75 | pwms[i].period_ms(1); |
kazuryu | 0:3dc012104243 | 76 | } |
kazuryu | 0:3dc012104243 | 77 | count_Move = 1; |
kazuryu | 0:3dc012104243 | 78 | } |
kazuryu | 0:3dc012104243 | 79 | for(int i =0;i<4;i++){ |
kazuryu | 0:3dc012104243 | 80 | num = Write[way][i]; |
kazuryu | 0:3dc012104243 | 81 | if(num == 255){ |
kazuryu | 0:3dc012104243 | 82 | pwms[i*2] = pwm*pwm_g[i]; |
kazuryu | 0:3dc012104243 | 83 | pwms[i*2+1] = 0; |
kazuryu | 0:3dc012104243 | 84 | }else if(num == -255){ |
kazuryu | 0:3dc012104243 | 85 | pwms[i*2] = 0; |
kazuryu | 0:3dc012104243 | 86 | pwms[i*2+1] = pwm*pwm_g[i]; |
kazuryu | 0:3dc012104243 | 87 | }else if(num == 500){ |
kazuryu | 4:9ba47e5db1e2 | 88 | pwms[i*2] = 0; |
kazuryu | 4:9ba47e5db1e2 | 89 | pwms[i*2+1] = 0; |
kazuryu | 0:3dc012104243 | 90 | } |
kazuryu | 0:3dc012104243 | 91 | } |
kazuryu | 0:3dc012104243 | 92 | } |
kazuryu | 4:9ba47e5db1e2 | 93 | /* |
kazuryu | 0:3dc012104243 | 94 | void Move_lock(int way,float pwm){ |
kazuryu | 0:3dc012104243 | 95 | int num; |
kazuryu | 0:3dc012104243 | 96 | if(count_Move_lock == 0){ |
kazuryu | 0:3dc012104243 | 97 | for(int i =0;i<2;i++){ |
kazuryu | 0:3dc012104243 | 98 | lockpwms[i].period_ms(1); |
kazuryu | 0:3dc012104243 | 99 | } |
kazuryu | 0:3dc012104243 | 100 | count_Move_lock = 1; |
kazuryu | 0:3dc012104243 | 101 | } |
kazuryu | 0:3dc012104243 | 102 | for(int i =0;i<1;i++){ |
kazuryu | 0:3dc012104243 | 103 | num = Write[way][i]; |
kazuryu | 0:3dc012104243 | 104 | if(num == 255){ |
kazuryu | 0:3dc012104243 | 105 | pwms[i*2] = pwm; |
kazuryu | 0:3dc012104243 | 106 | pwms[i*2+1] = 0; |
kazuryu | 0:3dc012104243 | 107 | }else if(num == -255){ |
kazuryu | 0:3dc012104243 | 108 | pwms[i*2] = 0; |
kazuryu | 0:3dc012104243 | 109 | pwms[i*2+1] = pwm; |
kazuryu | 0:3dc012104243 | 110 | } |
kazuryu | 0:3dc012104243 | 111 | } |
kazuryu | 0:3dc012104243 | 112 | } |
kazuryu | 4:9ba47e5db1e2 | 113 | */ |
kazuryu | 0:3dc012104243 | 114 | void trans(int a,int dis,float *x,float *y,int *sign){ |
kazuryu | 0:3dc012104243 | 115 | if(a == 0){ |
kazuryu | 0:3dc012104243 | 116 | *y=dis; |
kazuryu | 0:3dc012104243 | 117 | *x = 0; |
kazuryu | 0:3dc012104243 | 118 | }else if(a == 1){ |
kazuryu | 0:3dc012104243 | 119 | *y = -dis; |
kazuryu | 0:3dc012104243 | 120 | *x = 0; |
kazuryu | 0:3dc012104243 | 121 | }else if(a == 2){ |
kazuryu | 0:3dc012104243 | 122 | *y = 0; |
kazuryu | 0:3dc012104243 | 123 | *x = dis; |
kazuryu | 0:3dc012104243 | 124 | }else if(a == 3){ |
kazuryu | 0:3dc012104243 | 125 | *y = 0; |
kazuryu | 0:3dc012104243 | 126 | *x = -dis; |
kazuryu | 0:3dc012104243 | 127 | }else if(a == 4){ |
kazuryu | 0:3dc012104243 | 128 | *y = dis/1.414; |
kazuryu | 0:3dc012104243 | 129 | *x = dis/1.414; |
kazuryu | 0:3dc012104243 | 130 | }else if(a == 5){ |
kazuryu | 0:3dc012104243 | 131 | *y = dis/1.414; |
kazuryu | 0:3dc012104243 | 132 | *x = -dis/1.414; |
kazuryu | 0:3dc012104243 | 133 | }else if(a == 6){ |
kazuryu | 0:3dc012104243 | 134 | *y = -dis/1.414; |
kazuryu | 0:3dc012104243 | 135 | *x = -dis/1.414; |
kazuryu | 0:3dc012104243 | 136 | }else if(a == 7){ |
kazuryu | 0:3dc012104243 | 137 | *y = -dis/1.414; |
kazuryu | 0:3dc012104243 | 138 | *x = dis/1.414; |
kazuryu | 0:3dc012104243 | 139 | } |
kazuryu | 0:3dc012104243 | 140 | *sign = 0; |
kazuryu | 0:3dc012104243 | 141 | if(*x < 0)*sign = 0b10; |
kazuryu | 0:3dc012104243 | 142 | if(*y < 0)*sign = *sign | 0b01; |
kazuryu | 0:3dc012104243 | 143 | } |