mbedを用いた制御学生の制御
/
Kouryuu_reciverv1
アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。
main.cpp@13:1423effc49b2, 2015-03-17 (annotated)
- Committer:
- WAT34
- Date:
- Tue Mar 17 23:43:14 2015 +0000
- Revision:
- 13:1423effc49b2
- Parent:
- 12:12dca00d1bca
- Child:
- 14:54f198b9947b
?????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 0:2ac89e0419ac | 1 | #include "mbed.h" |
WAT34 | 12:12dca00d1bca | 2 | #include "HMC6352.h" |
WAT34 | 0:2ac89e0419ac | 3 | #define pi 3.141592653589793 |
WAT34 | 9:687dddda7432 | 4 | Serial sousin(p28,p27); |
WAT34 | 0:2ac89e0419ac | 5 | Serial pc(USBTX,USBRX); |
WAT34 | 0:2ac89e0419ac | 6 | Timer em; |
WAT34 | 12:12dca00d1bca | 7 | Timer t; |
WAT34 | 12:12dca00d1bca | 8 | HMC6352 sensor(p9,p10); |
WAT34 | 0:2ac89e0419ac | 9 | DigitalOut check (LED1); |
WAT34 | 1:252fd967389e | 10 | BusOut check2 (LED2,LED3); |
WAT34 | 11:00c82b2d3c0f | 11 | BusOut mo1 (p15,p16); |
WAT34 | 11:00c82b2d3c0f | 12 | BusOut mo2 (p17,p18); |
WAT34 | 11:00c82b2d3c0f | 13 | BusOut mo3 (p19,p20); |
WAT34 | 12:12dca00d1bca | 14 | BusIn hunter(p13,p14); |
WAT34 | 0:2ac89e0419ac | 15 | PwmOut mop1 (p21); |
WAT34 | 0:2ac89e0419ac | 16 | PwmOut mop2 (p22); |
WAT34 | 0:2ac89e0419ac | 17 | PwmOut mop3 (p23); |
WAT34 | 12:12dca00d1bca | 18 | |
WAT34 | 0:2ac89e0419ac | 19 | double get_theta(double stickx,double sticky) |
WAT34 | 0:2ac89e0419ac | 20 | { |
WAT34 | 0:2ac89e0419ac | 21 | double x,y,theta; |
WAT34 | 0:2ac89e0419ac | 22 | if (stickx>0.5){ |
WAT34 | 0:2ac89e0419ac | 23 | x = stickx - 0.5; |
WAT34 | 0:2ac89e0419ac | 24 | }else if(stickx <= 0.5){ |
WAT34 | 0:2ac89e0419ac | 25 | x = -(0.5 - stickx); |
WAT34 | 0:2ac89e0419ac | 26 | }else{ |
WAT34 | 0:2ac89e0419ac | 27 | x = 0; |
WAT34 | 0:2ac89e0419ac | 28 | } |
WAT34 | 0:2ac89e0419ac | 29 | if (sticky>0.5){ |
WAT34 | 0:2ac89e0419ac | 30 | y = sticky - 0.5; |
WAT34 | 0:2ac89e0419ac | 31 | }else if(sticky <= 0.5){ |
WAT34 | 0:2ac89e0419ac | 32 | y = -(0.5 - sticky); |
WAT34 | 0:2ac89e0419ac | 33 | }else{ |
WAT34 | 0:2ac89e0419ac | 34 | y = 0; |
WAT34 | 0:2ac89e0419ac | 35 | } |
WAT34 | 0:2ac89e0419ac | 36 | if(!(x==0 && y ==0)){ |
WAT34 | 0:2ac89e0419ac | 37 | theta =atan(y/x); |
WAT34 | 0:2ac89e0419ac | 38 | }else { |
WAT34 | 0:2ac89e0419ac | 39 | |
WAT34 | 0:2ac89e0419ac | 40 | theta = 0; |
WAT34 | 0:2ac89e0419ac | 41 | } |
WAT34 | 0:2ac89e0419ac | 42 | if(x>0 && y > 0){ |
WAT34 | 0:2ac89e0419ac | 43 | theta = pi+theta; |
WAT34 | 0:2ac89e0419ac | 44 | }else |
WAT34 | 0:2ac89e0419ac | 45 | if(x>0 && y < 0){ |
WAT34 | 0:2ac89e0419ac | 46 | theta = pi+theta; |
WAT34 | 0:2ac89e0419ac | 47 | }else |
WAT34 | 0:2ac89e0419ac | 48 | if(x<0 && y < 0){ |
WAT34 | 0:2ac89e0419ac | 49 | theta = theta; |
WAT34 | 0:2ac89e0419ac | 50 | }else{ |
WAT34 | 0:2ac89e0419ac | 51 | theta = 2*pi+theta; |
WAT34 | 0:2ac89e0419ac | 52 | } |
WAT34 | 0:2ac89e0419ac | 53 | return theta; |
WAT34 | 0:2ac89e0419ac | 54 | } |
WAT34 | 0:2ac89e0419ac | 55 | int get_serial(double *stx,double *sty,int8_t *sw) |
WAT34 | 0:2ac89e0419ac | 56 | { |
WAT34 | 0:2ac89e0419ac | 57 | int error,x,y; |
WAT34 | 0:2ac89e0419ac | 58 | if (sousin.getc() == 255) |
WAT34 | 0:2ac89e0419ac | 59 | { |
WAT34 | 0:2ac89e0419ac | 60 | x = sousin.getc(); |
WAT34 | 0:2ac89e0419ac | 61 | y = sousin.getc(); |
WAT34 | 0:2ac89e0419ac | 62 | *sw = sousin.getc(); |
WAT34 | 0:2ac89e0419ac | 63 | error = sousin.getc(); |
WAT34 | 0:2ac89e0419ac | 64 | *stx = x/100.0; |
WAT34 | 0:2ac89e0419ac | 65 | *sty = y/100.0; |
WAT34 | 0:2ac89e0419ac | 66 | if (error == x^y^(*sw)){ |
WAT34 | 0:2ac89e0419ac | 67 | em.reset(); |
WAT34 | 12:12dca00d1bca | 68 | //pc.printf("sw-->%d\n\r",x); |
WAT34 | 0:2ac89e0419ac | 69 | return 0; |
WAT34 | 0:2ac89e0419ac | 70 | }else{ |
WAT34 | 0:2ac89e0419ac | 71 | return 1; |
WAT34 | 0:2ac89e0419ac | 72 | } |
WAT34 | 0:2ac89e0419ac | 73 | }else{ |
WAT34 | 0:2ac89e0419ac | 74 | return 1; |
WAT34 | 0:2ac89e0419ac | 75 | } |
WAT34 | 0:2ac89e0419ac | 76 | } |
WAT34 | 0:2ac89e0419ac | 77 | void allstop() |
WAT34 | 0:2ac89e0419ac | 78 | { |
WAT34 | 0:2ac89e0419ac | 79 | mo1 = 0; |
WAT34 | 0:2ac89e0419ac | 80 | mo2 = 0; |
WAT34 | 0:2ac89e0419ac | 81 | mo3 = 0; |
WAT34 | 0:2ac89e0419ac | 82 | check = 1; |
WAT34 | 0:2ac89e0419ac | 83 | while(1){} |
WAT34 | 0:2ac89e0419ac | 84 | } |
WAT34 | 13:1423effc49b2 | 85 | void omni_cont(int hun,double aval,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3) |
WAT34 | 0:2ac89e0419ac | 86 | { |
WAT34 | 0:2ac89e0419ac | 87 | |
WAT34 | 0:2ac89e0419ac | 88 | double md1,md2,md3,t1; |
WAT34 | 0:2ac89e0419ac | 89 | if (hun == 2) |
WAT34 | 0:2ac89e0419ac | 90 | { |
WAT34 | 13:1423effc49b2 | 91 | t1 = (fabs(aval)/180)*0.3; |
WAT34 | 0:2ac89e0419ac | 92 | }else if(hun == 1) |
WAT34 | 0:2ac89e0419ac | 93 | { |
WAT34 | 13:1423effc49b2 | 94 | t1 = -(fabs(aval)/180)*0.3; |
WAT34 | 0:2ac89e0419ac | 95 | }else{ |
WAT34 | 0:2ac89e0419ac | 96 | t1 = 0; |
WAT34 | 0:2ac89e0419ac | 97 | } |
WAT34 | 13:1423effc49b2 | 98 | md1 = sin(theta)*0.7+t1; |
WAT34 | 13:1423effc49b2 | 99 | md2 = sin(theta-pi*2/3)*0.7+t1; |
WAT34 | 13:1423effc49b2 | 100 | md3 = sin(theta-pi*4/3)*0.7+t1; |
WAT34 | 0:2ac89e0419ac | 101 | if (md1 < 0){ |
WAT34 | 0:2ac89e0419ac | 102 | *m1 = 2; |
WAT34 | 0:2ac89e0419ac | 103 | *mp1 = -md1; |
WAT34 | 0:2ac89e0419ac | 104 | }else{ |
WAT34 | 0:2ac89e0419ac | 105 | *m1 = 1; |
WAT34 | 0:2ac89e0419ac | 106 | *mp1 = md1; |
WAT34 | 0:2ac89e0419ac | 107 | } |
WAT34 | 0:2ac89e0419ac | 108 | if (md2 < 0){ |
WAT34 | 0:2ac89e0419ac | 109 | *m2 = 2; |
WAT34 | 0:2ac89e0419ac | 110 | *mp2 = -md2; |
WAT34 | 0:2ac89e0419ac | 111 | }else{ |
WAT34 | 0:2ac89e0419ac | 112 | *m2 = 1; |
WAT34 | 0:2ac89e0419ac | 113 | *mp2 = md2; |
WAT34 | 0:2ac89e0419ac | 114 | } |
WAT34 | 0:2ac89e0419ac | 115 | if (md3 < 0){ |
WAT34 | 0:2ac89e0419ac | 116 | *m3 = 2; |
WAT34 | 0:2ac89e0419ac | 117 | *mp3 = -md3; |
WAT34 | 0:2ac89e0419ac | 118 | }else{ |
WAT34 | 0:2ac89e0419ac | 119 | *m3 = 1; |
WAT34 | 0:2ac89e0419ac | 120 | *mp3 = md3; |
WAT34 | 0:2ac89e0419ac | 121 | } |
WAT34 | 0:2ac89e0419ac | 122 | } |
WAT34 | 12:12dca00d1bca | 123 | |
WAT34 | 0:2ac89e0419ac | 124 | int main() { |
WAT34 | 0:2ac89e0419ac | 125 | sousin.baud(9600); |
WAT34 | 12:12dca00d1bca | 126 | sensor.setOpMode(HMC6352_CONTINUOUS,1,20); |
WAT34 | 13:1423effc49b2 | 127 | int i,ec,motor[3],val; |
WAT34 | 13:1423effc49b2 | 128 | double asen,sen,aval; |
WAT34 | 0:2ac89e0419ac | 129 | int8_t sw1,hun; |
WAT34 | 12:12dca00d1bca | 130 | i = 0; |
WAT34 | 12:12dca00d1bca | 131 | asen = sensor.sample()/10.0; |
WAT34 | 0:2ac89e0419ac | 132 | double sx,sy,motp[3],theta; |
WAT34 | 0:2ac89e0419ac | 133 | while(1) { |
WAT34 | 6:78538ae01ce6 | 134 | //pc.printf("START\n\r"); |
WAT34 | 13:1423effc49b2 | 135 | sen = sensor.sample()/10.0; |
WAT34 | 13:1423effc49b2 | 136 | //pc.printf("%f\n\r",sen); |
WAT34 | 12:12dca00d1bca | 137 | hun = hunter; |
WAT34 | 12:12dca00d1bca | 138 | check2 = hun; |
WAT34 | 0:2ac89e0419ac | 139 | ec = get_serial(&sx,&sy,&sw1); |
WAT34 | 12:12dca00d1bca | 140 | if (sw1 == -2){ |
WAT34 | 12:12dca00d1bca | 141 | hun = 1; |
WAT34 | 12:12dca00d1bca | 142 | } |
WAT34 | 11:00c82b2d3c0f | 143 | if (sw1 == -3){ |
WAT34 | 11:00c82b2d3c0f | 144 | hun =2; |
WAT34 | 12:12dca00d1bca | 145 | } |
WAT34 | 12:12dca00d1bca | 146 | ec =0; |
WAT34 | 12:12dca00d1bca | 147 | if(hun == 1||hun == 2){ |
WAT34 | 12:12dca00d1bca | 148 | t.start(); |
WAT34 | 12:12dca00d1bca | 149 | }else { |
WAT34 | 12:12dca00d1bca | 150 | t.reset(); |
WAT34 | 12:12dca00d1bca | 151 | t.stop(); |
WAT34 | 12:12dca00d1bca | 152 | } |
WAT34 | 13:1423effc49b2 | 153 | if ((hun == 1)&& (t.read_ms()>(i*20))){ |
WAT34 | 12:12dca00d1bca | 154 | asen = asen+1; |
WAT34 | 12:12dca00d1bca | 155 | i++; |
WAT34 | 13:1423effc49b2 | 156 | }else if((hun == 2)&& (t.read_ms()>(i*20))){ |
WAT34 | 12:12dca00d1bca | 157 | asen = asen-1; |
WAT34 | 12:12dca00d1bca | 158 | i++; |
WAT34 | 12:12dca00d1bca | 159 | } |
WAT34 | 12:12dca00d1bca | 160 | if (asen > 360){ |
WAT34 | 12:12dca00d1bca | 161 | asen = 0; |
WAT34 | 12:12dca00d1bca | 162 | } |
WAT34 | 12:12dca00d1bca | 163 | if (asen < 0){ |
WAT34 | 12:12dca00d1bca | 164 | asen = 360; |
WAT34 | 12:12dca00d1bca | 165 | } |
WAT34 | 13:1423effc49b2 | 166 | val = sen+540-asen; |
WAT34 | 13:1423effc49b2 | 167 | val %= 360; |
WAT34 | 13:1423effc49b2 | 168 | val -= 180; |
WAT34 | 13:1423effc49b2 | 169 | printf(" :%d\n\r",val); |
WAT34 | 13:1423effc49b2 | 170 | if (val > 10){ |
WAT34 | 13:1423effc49b2 | 171 | hun = 2; |
WAT34 | 13:1423effc49b2 | 172 | }else if (val <-10){ |
WAT34 | 11:00c82b2d3c0f | 173 | hun = 1; |
WAT34 | 13:1423effc49b2 | 174 | }else{ |
WAT34 | 13:1423effc49b2 | 175 | hun = 0; |
WAT34 | 13:1423effc49b2 | 176 | } |
WAT34 | 13:1423effc49b2 | 177 | aval = val; |
WAT34 | 13:1423effc49b2 | 178 | /*if(sen> asen-180){ |
WAT34 | 12:12dca00d1bca | 179 | hun = 2; |
WAT34 | 13:1423effc49b2 | 180 | }else if (sen< asen-180){ |
WAT34 | 13:1423effc49b2 | 181 | hun = 1; |
WAT34 | 12:12dca00d1bca | 182 | }else { |
WAT34 | 12:12dca00d1bca | 183 | hun = 0; |
WAT34 | 12:12dca00d1bca | 184 | } |
WAT34 | 13:1423effc49b2 | 185 | */ |
WAT34 | 12:12dca00d1bca | 186 | //pc.printf("asen-->%f",asen); |
WAT34 | 12:12dca00d1bca | 187 | ec = 0; |
WAT34 | 6:78538ae01ce6 | 188 | //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1); |
WAT34 | 6:78538ae01ce6 | 189 | //pc.printf("%d\n\r",hun); |
WAT34 | 0:2ac89e0419ac | 190 | if (ec == 0){ |
WAT34 | 0:2ac89e0419ac | 191 | em.reset(); |
WAT34 | 0:2ac89e0419ac | 192 | theta = get_theta(sx,sy); |
WAT34 | 12:12dca00d1bca | 193 | //pc.printf("%f\n\r",theta); |
WAT34 | 0:2ac89e0419ac | 194 | if (sw1 == -1){ |
WAT34 | 13:1423effc49b2 | 195 | omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]); |
WAT34 | 12:12dca00d1bca | 196 | mo1 = motor[0]; |
WAT34 | 12:12dca00d1bca | 197 | mo2 = motor[1]; |
WAT34 | 12:12dca00d1bca | 198 | mo3 = motor[2]; |
WAT34 | 12:12dca00d1bca | 199 | mop1 = motp[0]; |
WAT34 | 12:12dca00d1bca | 200 | mop2 = motp[1]; |
WAT34 | 12:12dca00d1bca | 201 | mop3 = motp[2]; |
WAT34 | 12:12dca00d1bca | 202 | |
WAT34 | 12:12dca00d1bca | 203 | }else |
WAT34 | 13:1423effc49b2 | 204 | if (hun == 2){ |
WAT34 | 12:12dca00d1bca | 205 | mo1 = 1; |
WAT34 | 12:12dca00d1bca | 206 | mo2 = 1; |
WAT34 | 12:12dca00d1bca | 207 | mo3 = 1; |
WAT34 | 13:1423effc49b2 | 208 | mop1 = (fabs(aval)/180); |
WAT34 | 13:1423effc49b2 | 209 | mop2 = (fabs(aval)/180); |
WAT34 | 13:1423effc49b2 | 210 | mop3 = (fabs(aval)/180); |
WAT34 | 13:1423effc49b2 | 211 | }else if(hun == 1){ |
WAT34 | 12:12dca00d1bca | 212 | mo1 = 2; |
WAT34 | 12:12dca00d1bca | 213 | mo2 = 2; |
WAT34 | 12:12dca00d1bca | 214 | mo3 = 2; |
WAT34 | 13:1423effc49b2 | 215 | mop1 = (fabs(aval)/180); |
WAT34 | 13:1423effc49b2 | 216 | mop2 = (fabs(aval)/180); |
WAT34 | 13:1423effc49b2 | 217 | mop3 = (fabs(aval)/180); |
WAT34 | 12:12dca00d1bca | 218 | }else { |
WAT34 | 12:12dca00d1bca | 219 | mo1 = 0; |
WAT34 | 12:12dca00d1bca | 220 | mo2 = 0; |
WAT34 | 12:12dca00d1bca | 221 | mo3 = 0; |
WAT34 | 12:12dca00d1bca | 222 | mop1 = 0; |
WAT34 | 12:12dca00d1bca | 223 | mop2 = 0; |
WAT34 | 12:12dca00d1bca | 224 | mop3 = 0; |
WAT34 | 0:2ac89e0419ac | 225 | } |
WAT34 | 0:2ac89e0419ac | 226 | }else{ |
WAT34 | 0:2ac89e0419ac | 227 | em.start(); |
WAT34 | 12:12dca00d1bca | 228 | |
WAT34 | 0:2ac89e0419ac | 229 | //if(em.read_ms()>200){ |
WAT34 | 0:2ac89e0419ac | 230 | // allstop(); |
WAT34 | 0:2ac89e0419ac | 231 | //} |
WAT34 | 0:2ac89e0419ac | 232 | } |
WAT34 | 0:2ac89e0419ac | 233 | } |
WAT34 | 0:2ac89e0419ac | 234 | } |