File content as of revision 3:3ff6832ca952:

/*講習者用コントローラープログラム
仕様
左アナログスティックでステアリングの角度
右アナログスティックで速度
*/
#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,dire;
    Xbee.baud(38400);
    while(1) {
        speedr = (0.7-RT)*360;
        speedl = (0.7-LT)*360;
        deg = getdeg();
        dire = getspeed();
        Xbee.putc(255);
        Xbee.putc(deg);
        Xbee.putc(speedr);
        Xbee.putc(speedl);
        Xbee.putc(dire);
        printf("%d,%d,%d\n",speedl,speedr,dire);
        wait(0.1);
    }
}