mbedを用いた制御学生の制御 / Mbed 2 deprecated Kouryuu_reciverv1

アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。

Dependencies:   HMC6352 mbed

main.cpp@5:854fd5676486, 2015-03-09 (annotated)

Committer:
WAT34
Date:
Mon Mar 09 05:42:54 2015 +0000
Revision:
5:854fd5676486
Parent:
4:941c51ddfdd8
Child:
6:78538ae01ce6
lcd??????

Who changed what in which revision?

UserRevisionLine numberNew 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 5:854fd5676486 4 Serial sousin(p28,p27);
WAT34 5:854fd5676486 5 Serial hunter(p13,p14);
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 5:854fd5676486 100 if (sousin.readable()){
WAT34 0:2ac89e0419ac 101 if (sousin.getc() == 255)
WAT34 0:2ac89e0419ac 102 {
WAT34 0:2ac89e0419ac 103 x = sousin.getc();
WAT34 0:2ac89e0419ac 104 y = sousin.getc();
WAT34 0:2ac89e0419ac 105 *sw = sousin.getc();
WAT34 0:2ac89e0419ac 106 error = sousin.getc();
WAT34 0:2ac89e0419ac 107 *stx = x/100.0;
WAT34 0:2ac89e0419ac 108 *sty = y/100.0;
WAT34 0:2ac89e0419ac 109 if (error == x^y^(*sw)){
WAT34 0:2ac89e0419ac 110 em.reset();
WAT34 1:252fd967389e 111 pc.printf("sw-->%d\n\r",x);
WAT34 0:2ac89e0419ac 112 return 0;
WAT34 0:2ac89e0419ac 113 }else{
WAT34 0:2ac89e0419ac 114 return 1;
WAT34 0:2ac89e0419ac 115 }
WAT34 0:2ac89e0419ac 116 }else{
WAT34 0:2ac89e0419ac 117 return 1;
WAT34 0:2ac89e0419ac 118 }
WAT34 5:854fd5676486 119 }
WAT34 0:2ac89e0419ac 120 }
WAT34 0:2ac89e0419ac 121 void allstop()
WAT34 0:2ac89e0419ac 122 {
WAT34 0:2ac89e0419ac 123 mo1 = 0;
WAT34 0:2ac89e0419ac 124 mo2 = 0;
WAT34 0:2ac89e0419ac 125 mo3 = 0;
WAT34 0:2ac89e0419ac 126 check = 1;
WAT34 0:2ac89e0419ac 127 while(1){}
WAT34 0:2ac89e0419ac 128 }
WAT34 3:a3a797eb7525 129 void omni_cont(int hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
WAT34 0:2ac89e0419ac 130 {
WAT34 0:2ac89e0419ac 131
WAT34 0:2ac89e0419ac 132 double md1,md2,md3,t1;
WAT34 0:2ac89e0419ac 133 if (hun == 2)
WAT34 0:2ac89e0419ac 134 {
WAT34 3:a3a797eb7525 135 t1 = 0.6;
WAT34 0:2ac89e0419ac 136 }else if(hun == 1)
WAT34 0:2ac89e0419ac 137 {
WAT34 3:a3a797eb7525 138 t1 = -0.6;
WAT34 0:2ac89e0419ac 139 }else{
WAT34 0:2ac89e0419ac 140 t1 = 0;
WAT34 0:2ac89e0419ac 141 }
WAT34 3:a3a797eb7525 142 md1 = sin(theta)*0.4+t1;
WAT34 3:a3a797eb7525 143 md2 = sin(theta-pi*2/3)*0.4+t1;
WAT34 3:a3a797eb7525 144 md3 = sin(theta-pi*4/3)*0.4+t1;
WAT34 0:2ac89e0419ac 145 if (md1 < 0){
WAT34 0:2ac89e0419ac 146 *m1 = 2;
WAT34 0:2ac89e0419ac 147 *mp1 = -md1;
WAT34 0:2ac89e0419ac 148 }else{
WAT34 0:2ac89e0419ac 149 *m1 = 1;
WAT34 0:2ac89e0419ac 150 *mp1 = md1;
WAT34 0:2ac89e0419ac 151 }
WAT34 0:2ac89e0419ac 152 if (md2 < 0){
WAT34 0:2ac89e0419ac 153 *m2 = 2;
WAT34 0:2ac89e0419ac 154 *mp2 = -md2;
WAT34 0:2ac89e0419ac 155 }else{
WAT34 0:2ac89e0419ac 156 *m2 = 1;
WAT34 0:2ac89e0419ac 157 *mp2 = md2;
WAT34 0:2ac89e0419ac 158 }
WAT34 0:2ac89e0419ac 159 if (md3 < 0){
WAT34 0:2ac89e0419ac 160 *m3 = 2;
WAT34 0:2ac89e0419ac 161 *mp3 = -md3;
WAT34 0:2ac89e0419ac 162 }else{
WAT34 0:2ac89e0419ac 163 *m3 = 1;
WAT34 0:2ac89e0419ac 164 *mp3 = md3;
WAT34 0:2ac89e0419ac 165 }
WAT34 0:2ac89e0419ac 166 }
WAT34 0:2ac89e0419ac 167 int8_t get_hunter()
WAT34 0:2ac89e0419ac 168 {
WAT34 0:2ac89e0419ac 169 int8_t s;
WAT34 1:252fd967389e 170 if (hunter.readable()){
WAT34 1:252fd967389e 171 if (hunter.getc() == 48){
WAT34 1:252fd967389e 172 s = hunter.getc();
WAT34 1:252fd967389e 173 }
WAT34 0:2ac89e0419ac 174 }
WAT34 0:2ac89e0419ac 175 if (s == 255)
WAT34 0:2ac89e0419ac 176 {
WAT34 0:2ac89e0419ac 177 allstop();
WAT34 0:2ac89e0419ac 178 }
WAT34 0:2ac89e0419ac 179 pc.printf("s-->%d\n\r",s);
WAT34 0:2ac89e0419ac 180 return s;
WAT34 0:2ac89e0419ac 181 }
WAT34 0:2ac89e0419ac 182 int main() {
WAT34 3:a3a797eb7525 183 t.start();
WAT34 0:2ac89e0419ac 184 hunter.baud(9600);
WAT34 0:2ac89e0419ac 185 sousin.baud(9600);
WAT34 3:a3a797eb7525 186 int i,ec,motor[3],sen,asen;
WAT34 3:a3a797eb7525 187 asen = get_sen();
WAT34 0:2ac89e0419ac 188 int8_t sw1,hun;
WAT34 3:a3a797eb7525 189 i = 0;
WAT34 0:2ac89e0419ac 190 double sx,sy,motp[3],theta;
WAT34 0:2ac89e0419ac 191 while(1) {
WAT34 5:854fd5676486 192 pc.printf("START\n\r");
WAT34 3:a3a797eb7525 193 sen = get_sen();
WAT34 0:2ac89e0419ac 194 hun = get_hunter();
WAT34 4:941c51ddfdd8 195 lcd.home();
WAT34 4:941c51ddfdd8 196 lcd.printf("%f\n\r%d",sen,hun);
WAT34 1:252fd967389e 197 check2 = hun;
WAT34 0:2ac89e0419ac 198 ec = get_serial(&sx,&sy,&sw1);
WAT34 5:854fd5676486 199 ec = 0;
WAT34 4:941c51ddfdd8 200 if ((hun == 1)&& (t.read_ms()>(i*200))){
WAT34 3:a3a797eb7525 201 asen = asen+1;
WAT34 3:a3a797eb7525 202 i++;
WAT34 4:941c51ddfdd8 203 }else if((hun == 2)&& (t.read_ms()>(i*200))){
WAT34 3:a3a797eb7525 204 asen += -1;
WAT34 3:a3a797eb7525 205 i++;
WAT34 3:a3a797eb7525 206 }
WAT34 3:a3a797eb7525 207 if (asen > 360){
WAT34 3:a3a797eb7525 208 asen = 0;
WAT34 3:a3a797eb7525 209 }
WAT34 3:a3a797eb7525 210 if (asen < 0){
WAT34 3:a3a797eb7525 211 asen = 360;
WAT34 3:a3a797eb7525 212 }
WAT34 3:a3a797eb7525 213 if (sen > asen+10){
WAT34 3:a3a797eb7525 214 hun = 1;
WAT34 3:a3a797eb7525 215 }else if (sen <asen-10){
WAT34 3:a3a797eb7525 216 hun = 2;
WAT34 3:a3a797eb7525 217 }else {
WAT34 3:a3a797eb7525 218 hun = 0;
WAT34 3:a3a797eb7525 219 }
WAT34 5:854fd5676486 220 pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1);
WAT34 5:854fd5676486 221 pc.printf("%d\n\r",hun);
WAT34 0:2ac89e0419ac 222 if (ec == 0){
WAT34 0:2ac89e0419ac 223 em.reset();
WAT34 0:2ac89e0419ac 224 theta = get_theta(sx,sy);
WAT34 0:2ac89e0419ac 225 if (sw1 == -1){
WAT34 0:2ac89e0419ac 226 omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
WAT34 0:2ac89e0419ac 227 }else{
WAT34 0:2ac89e0419ac 228 motor[0] = 0;
WAT34 0:2ac89e0419ac 229 motor[1] = 0;
WAT34 0:2ac89e0419ac 230 motor[2] = 0;
WAT34 0:2ac89e0419ac 231 motp[0] = 0;
WAT34 0:2ac89e0419ac 232 motp[1] = 0;
WAT34 0:2ac89e0419ac 233 motp[2] = 0;
WAT34 0:2ac89e0419ac 234 }
WAT34 0:2ac89e0419ac 235 mo1 = motor[0];
WAT34 0:2ac89e0419ac 236 mo2 = motor[1];
WAT34 0:2ac89e0419ac 237 mo3 = motor[2];
WAT34 0:2ac89e0419ac 238 mop1 = motp[0];
WAT34 0:2ac89e0419ac 239 mop2 = motp[1];
WAT34 0:2ac89e0419ac 240 mop3 = motp[2];
WAT34 0:2ac89e0419ac 241 }else{
WAT34 0:2ac89e0419ac 242 em.start();
WAT34 3:a3a797eb7525 243 if (hun == 1){
WAT34 3:a3a797eb7525 244 mo1 = 1;
WAT34 3:a3a797eb7525 245 mo2 = 1;
WAT34 3:a3a797eb7525 246 mo3 = 1;
WAT34 3:a3a797eb7525 247 mop1 = 1;
WAT34 3:a3a797eb7525 248 mop2 = 1;
WAT34 3:a3a797eb7525 249 mop3 = 1;
WAT34 3:a3a797eb7525 250 }else if(hun == 2){
WAT34 3:a3a797eb7525 251 mo1 = 2;
WAT34 3:a3a797eb7525 252 mo2 = 2;
WAT34 3:a3a797eb7525 253 mo3 = 2;
WAT34 3:a3a797eb7525 254 mop1 = 1;
WAT34 3:a3a797eb7525 255 mop2 = 1;
WAT34 3:a3a797eb7525 256 mop3 = 1;
WAT34 3:a3a797eb7525 257 }else {
WAT34 3:a3a797eb7525 258 mo1 = 0;
WAT34 3:a3a797eb7525 259 mo2 = 0;
WAT34 3:a3a797eb7525 260 mo3 = 0;
WAT34 3:a3a797eb7525 261 mop1 = 0;
WAT34 3:a3a797eb7525 262 mop2 = 0;
WAT34 3:a3a797eb7525 263 mop3 = 0;
WAT34 3:a3a797eb7525 264 }
WAT34 0:2ac89e0419ac 265 //if(em.read_ms()>200){
WAT34 0:2ac89e0419ac 266 // allstop();
WAT34 0:2ac89e0419ac 267 //}
WAT34 0:2ac89e0419ac 268 }
WAT34 0:2ac89e0419ac 269 }
WAT34 0:2ac89e0419ac 270 }