mbedを用いた制御学生の制御
/
Kouryuu_reciverv1
アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。
main.cpp
- Committer:
- WAT34
- Date:
- 2015-03-09
- Revision:
- 3:a3a797eb7525
- Parent:
- 1:252fd967389e
- Child:
- 4:941c51ddfdd8
File content as of revision 3:a3a797eb7525:
#include "mbed.h" #define pi 3.141592653589793 Serial sousin(p13,p14); Serial hunter(p28,p27); Serial pc(USBTX,USBRX); Timer em; Timer t; I2C sensor(p9,p10); DigitalOut check (LED1); BusOut check2 (LED2,LED3); BusOut mo1 (p15,p16); BusOut mo2 (p17,p18); BusOut mo3 (p19,p20); PwmOut mop1 (p21); PwmOut mop2 (p22); PwmOut mop3 (p23); double get_sen(void) { double degree; double phi,theta,tp; double val[2]; int i = 0; short d[2]; char addr = 0xD0; char raddr = 0xD1; char maddr = 0x18; char mraddr = 0x19; sensor.frequency(400000); char put1[] = {0x6b,0x00}; char put2[] = {0x37,0x02}; char put3[] = {0x0a,0x12}; char put4[] = {0x03}; char data[6]; sensor.write(addr,put1,2); sensor.write(addr,put2,2); sensor.write(maddr,put3,2); sensor.write(maddr,put4,1); sensor.read(mraddr,data,7); while(i<= 2){ d[i] = data[2*i] + data[2*i+1]<<8; i++; } val[0] = d[0] *0.15; val[1] = d[1] *0.15; val[2] = d[2] *0.15; tp = sqrt(val[0]*val[0]+val[1]*val[1]+val[2]*val[2]); theta = atan2(val[0],val[1]); phi = acos(val[2]/tp); //printf("theta-->%f phi--->%f\n\r",theta,phi); if(theta>=0) { degree = theta/pi*180; }else{ degree = (pi+theta)/pi*180+180; } //printf("degree-->%d",degree); return degree; } double get_theta(double stickx,double sticky) { double x,y,theta; if (stickx>0.5){ x = stickx - 0.5; }else if(stickx <= 0.5){ x = -(0.5 - stickx); }else{ x = 0; } if (sticky>0.5){ y = sticky - 0.5; }else if(sticky <= 0.5){ y = -(0.5 - sticky); }else{ y = 0; } if(!(x==0 && y ==0)){ theta =atan(y/x); }else { theta = 0; } if(x>0 && y > 0){ theta = pi+theta; }else if(x>0 && y < 0){ theta = pi+theta; }else if(x<0 && y < 0){ theta = theta; }else{ theta = 2*pi+theta; } return theta; } int get_serial(double *stx,double *sty,int8_t *sw) { int error,x,y; if (sousin.getc() == 255) { x = sousin.getc(); y = sousin.getc(); *sw = sousin.getc(); error = sousin.getc(); *stx = x/100.0; *sty = y/100.0; if (error == x^y^(*sw)){ em.reset(); pc.printf("sw-->%d\n\r",x); return 0; }else{ return 1; } }else{ return 1; } } void allstop() { mo1 = 0; mo2 = 0; mo3 = 0; check = 1; while(1){} } void omni_cont(int hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3) { double md1,md2,md3,t1; if (hun == 2) { t1 = 0.6; }else if(hun == 1) { t1 = -0.6; }else{ t1 = 0; } md1 = sin(theta)*0.4+t1; md2 = sin(theta-pi*2/3)*0.4+t1; md3 = sin(theta-pi*4/3)*0.4+t1; if (md1 < 0){ *m1 = 2; *mp1 = -md1; }else{ *m1 = 1; *mp1 = md1; } if (md2 < 0){ *m2 = 2; *mp2 = -md2; }else{ *m2 = 1; *mp2 = md2; } if (md3 < 0){ *m3 = 2; *mp3 = -md3; }else{ *m3 = 1; *mp3 = md3; } } int8_t get_hunter() { int8_t s; if (hunter.readable()){ if (hunter.getc() == 48){ s = hunter.getc(); } } if (s == 255) { allstop(); } pc.printf("s-->%d\n\r",s); return s; } int main() { t.start(); hunter.baud(9600); sousin.baud(9600); int i,ec,motor[3],sen,asen; asen = get_sen(); int8_t sw1,hun; i = 0; double sx,sy,motp[3],theta; while(1) { //pc.printf("START\n\r"); sen = get_sen(); hun = get_hunter(); check2 = hun; ec = get_serial(&sx,&sy,&sw1); ec =0; if ((hun = 1)&& (t.read_ms()>(i*200))){ asen = asen+1; i++; }else if((hun = 2)&& (t.read_ms()>(i*200))){ asen += -1; i++; } if (asen > 360){ asen = 0; } if (asen < 0){ asen = 360; } if (sen > asen+10){ hun = 1; }else if (sen <asen-10){ hun = 2; }else { hun = 0; } //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1); //pc.printf("%d\n\r",hun); if (ec == 0){ em.reset(); theta = get_theta(sx,sy); if (sw1 == -1){ omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]); }else{ motor[0] = 0; motor[1] = 0; motor[2] = 0; motp[0] = 0; motp[1] = 0; motp[2] = 0; } mo1 = motor[0]; mo2 = motor[1]; mo3 = motor[2]; mop1 = motp[0]; mop2 = motp[1]; mop3 = motp[2]; }else{ em.start(); if (hun == 1){ mo1 = 1; mo2 = 1; mo3 = 1; mop1 = 1; mop2 = 1; mop3 = 1; }else if(hun == 2){ mo1 = 2; mo2 = 2; mo3 = 2; mop1 = 1; mop2 = 1; mop3 = 1; }else { mo1 = 0; mo2 = 0; mo3 = 0; mop1 = 0; mop2 = 0; mop3 = 0; } //if(em.read_ms()>200){ // allstop(); //} } } }