Revision 4:3408903ad725, committed 2016-03-14

Comitter:

tknara

Date:

Mon Mar 14 15:48:00 2016 +0000

Parent:

3:3ff6832ca952

Child:

5:714f1a772584

Commit message:

no rote

Changed in this revision

--- a/main.cpp	Mon Mar 14 09:44:25 2016 +0000
+++ b/main.cpp	Mon Mar 14 15:48:00 2016 +0000
@@ -7,6 +7,10 @@
 #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
 /*header list */
 #define turn 0xFF
 #define velocity 0xFA
@@ -19,30 +23,34 @@
 AnalogIn RT(p19);
 AnalogIn LT(p20);
 Serial pc(USBTX,USBRX);
-uint8_t getdeg(){
+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;
+        x=(1-x)*xmagp;
         counter +=1;
     }else if(1.1<x){
-        x=(x-1)*-1;
+        x=((x-1)*-1)*xmagm;
         counter +=2;
     }if((0.1>x)&&(-0.1<x)){
         x = 0; 
     }
     if(1.1>y){
-        y=1-y;
+        y=(1-y)*ymagp;
         counter +=4;
     }else if(1.1<y){
-        y=(y-1)*-1;
+        y=((y-1)*-1)*ymagm;
         counter +=8;
     }if((0.1>y)&&(-0.1<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;
@@ -69,34 +77,18 @@
     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);
+    uint8_t deg,dire2;
+    float dire1;
+    Xbee.baud(9600);
     while(1) {
-        speedr = (0.7-RT)*360;
-        speedl = (0.7-LT)*360;
-        deg = getdeg();
-        dire = getspeed();
+        deg = getdeg(&dire1);
+        dire2=255*dire1;
         Xbee.putc(255);
         Xbee.putc(deg);
-        Xbee.putc(speedr);
-        Xbee.putc(speedl);
-        Xbee.putc(dire);
-        printf("%d,%d,%d\n",speedl,speedr,dire);
+        Xbee.putc(dire2);
+        printf("%d,%d\n",deg,dire2);
         wait(0.1);
     }
 }