main.cpp

00001 /*講習者用コントローラープログラム
00002 仕様
00003 左アナログスティックでステアリングの角度
00004 右アナログスティックで速度
00005 */
00006 #include "mbed.h"
00007 #include "ACM1602.h"
00008 #define pi 3.14
00009 #define rad pi/180
00010 #define xmagp  1.65
00011 #define xmagm  1.33
00012 #define ymagp  1.38
00013 #define ymagm  1.75
00014 #define over   0.2
00015 #define under  -0.2
00016 /*header list */
00017 #define turn 0xFF
00018 #define velocity 0xFA
00019 ACM1602 lcd(p28,p27,0xa0);
00020 Serial Xbee(p13,p14);//tx rx
00021 BusOut led(LED1,LED2,LED3,LED4);
00022 AnalogIn sticky(p15);
00023 AnalogIn stickx(p16);
00024 AnalogIn stickx2(p17);
00025 AnalogIn sticky2(p18);
00026 AnalogIn RT(p19);
00027 AnalogIn LT(p20);
00028 Serial pc(USBTX,USBRX);
00029 uint8_t GetAngle()
00030 uint8_t getdeg(float *p){
00031     int counter = 0;
00032     float x=0,y=0,theta=0,deg=0;
00033     x = stickx*2;
00034     y = sticky*2;
00035     //printf("X == %f Y == %f\n",x,y);
00036     if(1.1>x){
00037         x=(1-x)*xmagp;
00038         counter +=1;
00039     }else if(1.1<x){
00040         x=((x-1)*-1)*xmagm;
00041         counter +=2;
00042     }if((over>x)&&(under<x)){
00043         x = 0; 
00044     }
00045     if(1.1>y){
00046         y=(1-y)*ymagp;
00047         counter +=4;
00048     }else if(1.1<y){
00049         y=((y-1)*-1)*ymagm;
00050         counter +=8;
00051     }if((over>y)&&(under<y)){
00052         y = 0;
00053     }
00054     *p = x*x+y*y;
00055     if(*p>1){
00056         *p = 1;
00057     }
00058     //printf("X == %f Y == %f\n",x,y);
00059     theta = atan(y/x);
00060     deg = theta * 180/pi;
00061     //printf("%f\n",deg);
00062     if(counter == 5){
00063         deg = (deg-90)*-1;
00064         led = 1;
00065     }
00066     else if ((counter == 6)||(counter == 10)){
00067         deg = 270-deg;
00068         led = 2;
00069     }
00070     else if(counter == 9){
00071         deg =90-deg;
00072         led = 4;
00073     }
00074     else{
00075         deg = 0;
00076         led = 8;
00077     }
00078     if((x==0)&&(y==0)){
00079         deg = 360;
00080     }
00081     deg = ((uint8_t)((deg/10)+0.5))*10;
00082     return deg/10;
00083 }
00084 
00085 int main() {
00086     uint8_t deg,dire2,right,left;
00087     float dire1;
00088     Xbee.baud(19200);
00089     while(1) {
00090         right = RT*255;
00091         left  = LT*255;
00092         deg = getdeg(&dire1);
00093         dire2=255*dire1;
00094         Xbee.putc(255);
00095         Xbee.putc(deg);
00096         Xbee.putc(right);
00097         Xbee.putc(left);
00098         /*
00099         Xbee.putc('\r');
00100         Xbee.putc('\n');
00101         */
00102         printf("%d,%d,%d,%d\n",deg,dire2,right,left);
00103         wait(0.1);
00104     }
00105 }