mbedを用いた制御学生の制御
/
Kouryuu_reciverv1
アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。
main.cpp
- Committer:
- WAT34
- Date:
- 2015-03-17
- Revision:
- 12:12dca00d1bca
- Parent:
- 11:00c82b2d3c0f
- Child:
- 13:1423effc49b2
File content as of revision 12:12dca00d1bca:
#include "mbed.h" #include "HMC6352.h" #define pi 3.141592653589793 Serial sousin(p28,p27); Serial pc(USBTX,USBRX); Timer em; Timer t; HMC6352 sensor(p9,p10); DigitalOut check (LED1); BusOut check2 (LED2,LED3); BusOut mo1 (p15,p16); BusOut mo2 (p17,p18); BusOut mo3 (p19,p20); BusIn hunter(p13,p14); PwmOut mop1 (p21); PwmOut mop2 (p22); PwmOut mop3 (p23); 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; } } int main() { sousin.baud(9600); sensor.setOpMode(HMC6352_CONTINUOUS,1,20); int i,ec,motor[3],sen; double asen; int8_t sw1,hun; i = 0; asen = sensor.sample()/10.0; double sx,sy,motp[3],theta; while(1) { //pc.printf("START\n\r"); hun = hunter; check2 = hun; ec = get_serial(&sx,&sy,&sw1); if (sw1 == -2){ hun = 1; } if (sw1 == -3){ hun =2; } ec =0; if(hun == 1||hun == 2){ t.start(); }else { t.reset(); t.stop(); } if ((hun == 1)&& (t.read_ms()>(i*200))){ asen = asen+1; i++; }else if((hun == 2)&& (t.read_ms()>(i*200))){ asen = asen-1; i++; } if (asen > 360){ asen = 0; } if (asen < 0){ asen = 360; } if (sen > asen+20){ hun = 1; }else if (sen <asen-20){ hun = 2; }else { hun = 0; } //pc.printf("asen-->%f",asen); ec = 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); //pc.printf("%f\n\r",theta); if (sw1 == -1){ omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]); mo1 = motor[0]; mo2 = motor[1]; mo3 = motor[2]; mop1 = motp[0]; mop2 = motp[1]; mop3 = motp[2]; }else if (hun == 1){ mo1 = 1; mo2 = 1; mo3 = 1; mop1 = 0.5; mop2 = 0.5; mop3 = 0.5; }else if(hun == 2){ mo1 = 2; mo2 = 2; mo3 = 2; mop1 = 0.5; mop2 = 0.5; mop3 = 0.5; }else { mo1 = 0; mo2 = 0; mo3 = 0; mop1 = 0; mop2 = 0; mop3 = 0; } }else{ em.start(); //if(em.read_ms()>200){ // allstop(); //} } } }