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);
    }
}