T Nara
/
robottt
ok
main.cpp
- Committer:
- tknara
- Date:
- 2016-01-28
- Revision:
- 2:06791a891865
- Parent:
- 1:daff0eea2103
- Child:
- 3:3ff6832ca952
File content as of revision 2:06791a891865:
/*講習者用コントローラープログラム 仕様 左アナログスティックでステアリングの角度 右アナログスティックで速度 */ #include "mbed.h" #include "ACM1602.h" #define pi 3.14 #define rad pi/180 /*header list */ #define turn 0xFF #define velocity 0xFA ACM1602 lcd(p28,p27,0xa0); Serial Xbee(p13,p14);//tx rx BusOut led(LED1,LED2,LED3,LED4); AnalogIn sticky(p15); AnalogIn stickx(p16); AnalogIn speed(p17); AnalogIn RT(p19); AnalogIn LT(p20); Serial pc(USBTX,USBRX); uint8_t getdeg(){ int counter = 0; float x=0,y=0,theta=0,deg=0; x = stickx*2; y = sticky*2; //printf("X == %f Y == %f\n",x,y); if(1.1>x){ x=1-x; counter +=1; }else if(1.1<x){ x=(x-1)*-1; counter +=2; }if((0.1>x)&&(-0.1<x)){ x = 0; } if(1.1>y){ y=1-y; counter +=4; }else if(1.1<y){ y=(y-1)*-1; counter +=8; }if((0.1>y)&&(-0.1<y)){ y = 0; } //printf("X == %f Y == %f\n",x,y); theta = atan(y/x); deg = theta * 180/pi; //printf("%f\n",deg); if(counter == 5){ deg = (deg-90)*-1; led = 1; } else if ((counter == 6)||(counter == 10)){ deg = 270-deg; led = 2; } else if(counter == 9){ deg =90-deg; led = 4; } else{ deg = 0; led = 8; } if((x==0)&&(y==0)){ deg = 360; } deg = ((uint8_t)((deg/10)+0.5))*10; return deg/10; } uint8_t getspeed(){ float x; x = speed*2; if(1.1>x){ x=(1-x)*1/0.64; }else if(1.1<x){ x=(x-1)*-1*1/0.71; }if((0.1>x)&&(-0.1<x)){ x = 0; } x = (uint8_t)((0.5+x*0.5)*250); //printf("%f\n",x); return x; } int main() { uint8_t deg,speedr,speedl; while(1) { speedr = (0.7-RT)*360; speedl = (0.7-LT)*360; deg = getdeg(); //speed = getspeed(); Xbee.putc(255); Xbee.putc(deg); Xbee.putc(speedr); Xbee.putc(speedl); //printf("%d,%d\n",speedl,speedr); wait(0.1); } }