T Nara
/
robottt
ok
main.cpp
- Committer:
- tknara
- Date:
- 2017-01-27
- Revision:
- 6:1c68c8eb8082
- Parent:
- 5:714f1a772584
File content as of revision 6:1c68c8eb8082:
/*講習者用コントローラープログラム 仕様 左アナログスティックでステアリングの角度 右アナログスティックで速度 */ #include "mbed.h" #include "ACM1602.h" #define pi 3.14 #define rad pi/180 #define xmagp 1.65 #define xmagm 1.33 #define ymagp 1.38 #define ymagm 1.75 #define over 0.2 #define under -0.2 /*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 stickx2(p17); AnalogIn sticky2(p18); AnalogIn RT(p19); AnalogIn LT(p20); Serial pc(USBTX,USBRX); uint8_t GetAngle() uint8_t getdeg(float *p){ 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)*xmagp; counter +=1; }else if(1.1<x){ x=((x-1)*-1)*xmagm; counter +=2; }if((over>x)&&(under<x)){ x = 0; } if(1.1>y){ y=(1-y)*ymagp; counter +=4; }else if(1.1<y){ y=((y-1)*-1)*ymagm; counter +=8; }if((over>y)&&(under<y)){ y = 0; } *p = x*x+y*y; if(*p>1){ *p = 1; } //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; } int main() { uint8_t deg,dire2,right,left; float dire1; Xbee.baud(19200); while(1) { right = RT*255; left = LT*255; deg = getdeg(&dire1); dire2=255*dire1; Xbee.putc(255); Xbee.putc(deg); Xbee.putc(right); Xbee.putc(left); /* Xbee.putc('\r'); Xbee.putc('\n'); */ printf("%d,%d,%d,%d\n",deg,dire2,right,left); wait(0.1); } }