T Nara
/
robottt
ok
Diff: main.cpp
- Revision:
- 1:daff0eea2103
- Parent:
- 0:36d4a66995e6
- Child:
- 2:06791a891865
diff -r 36d4a66995e6 -r daff0eea2103 main.cpp --- a/main.cpp Sun Mar 22 05:19:58 2015 +0000 +++ b/main.cpp Sat Jan 23 08:12:01 2016 +0000 @@ -1,20 +1,95 @@ -/* -このプログラムはアナログスティックの角度を求めることを目的とする +/*講習者用コントローラープログラム +仕様 +左アナログスティックでステアリングの角度 +右アナログスティックで速度 */ #include "mbed.h" -#define pi 3.141592 - +#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 sticky(p17); +AnalogIn speed(p17); 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() { - float x,y,theta,deg; + uint8_t deg,speed; while(1) { - x = stickx; - y = sticky; - theta = atan(x/y); - deg = theta * 180 / pi; - printf("deg<<%f\n",deg); - wait(1); + deg = getdeg(); + speed = getspeed(); + Xbee.putc(255); + Xbee.putc(deg); + Xbee.putc(speed); + printf("%d\n",deg); + wait(0.1); } }