mbedを用いた制御学生の制御 / Mbed 2 deprecated Kouryuu_reciverv1

アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。

Dependencies:   HMC6352 mbed

Diff: main.cpp

Revision:
14:54f198b9947b
Parent:
13:1423effc49b2
Child:
15:02d4a5eed16c
--- a/main.cpp	Tue Mar 17 23:43:14 2015 +0000
+++ b/main.cpp	Wed Mar 18 00:19:43 2015 +0000
@@ -125,10 +125,11 @@
     sousin.baud(9600);
     sensor.setOpMode(HMC6352_CONTINUOUS,1,20);
     int i,ec,motor[3],val;
-    double asen,sen,aval;
+    double asen,asen2,sen,aval;
     int8_t sw1,hun;
     i = 0;
     asen = sensor.sample()/10.0;
+    asen2 = asen;
     double sx,sy,motp[3],theta;
     while(1) {
         //pc.printf("START\n\r");
@@ -150,11 +151,11 @@
             t.reset();
             t.stop();
         }
-        if ((hun == 1)&& (t.read_ms()>(i*20))){
-            asen = asen+1;
+        if ((hun == 1)&& (t.read_ms()>(i*10))){
+            asen = asen+2;
             i++;
-        }else if((hun == 2)&& (t.read_ms()>(i*20))){
-            asen = asen-1;
+        }else if((hun == 2)&& (t.read_ms()>(i*10))){
+            asen = asen-2;
             i++;
         }
         if (asen > 360){
@@ -167,9 +168,9 @@
         val %= 360;
         val -= 180;
         printf("   :%d\n\r",val);
-        if (val > 10){
+        if (val > 5){
             hun = 2;
-        }else if (val <-10){
+        }else if (val <-5){
             hun = 1;
         }else{
             hun = 0;
@@ -190,6 +191,7 @@
         if (ec == 0){
             em.reset();
             theta = get_theta(sx,sy);
+            theta -=asen2;
             //pc.printf("%f\n\r",theta);
             if (sw1 == -1){
                 omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);