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