Diff: main.cpp

Revision:

1:daff0eea2103

Parent:

0:36d4a66995e6

Child:

2:06791a891865

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