NagaokaRoboticsClub_mbedTeam / Mbed OS quadOmni_yanagi

use for experiment before the demonstration at open-campus

Dependencies:   FEP ikarashiMDC omni PID R1370

Fork of omni_sample by NagaokaRoboticsClub_mbedTeam

Files at this revision

API Documentation at this revision

Revision 16:7eaf3343f45e, committed 2017-09-15

Comitter:
tkNara
Date:
Fri Sep 15 10:14:41 2017 +0900
Parent:
15:d4ff132a616d
Commit message:
update FEP baud 115200

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Thu Sep 14 18:22:29 2017 +0900
+++ b/main.cpp	Fri Sep 15 10:14:41 2017 +0900
@@ -60,7 +60,7 @@
 {
     bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
     int error_val = 0,i = 0;
-    float nowAngle = 0;//,oldAngle = 0;
+    float nowAngle = 0,rote = 0.0;//,oldAngle = 0;
     init();
     pidInit();
     gyroInit();
@@ -75,19 +75,37 @@
                 } else {
                     powerSw = 1;
                 }
+                for(i=0;i<6;i++){
+                    pc.printf("%d,%d\n",i,con.getButton2(i));
+                }
                 error_val = 0;
                 nowAngle = R1370.getAngle();
-                if((con.getStick(0)==0)&&(pidflag==1)) {
+                /*if((con.getStick(0)==0)&&(pidflag==1)) {
                     pc.printf("zeroPointSet\n");
                     pidflag = 0;
                     R1370.setZeroPoint(nowAngle);
                 }else if(con.getStick(0)!=0) {
                     pidflag = 1;
-                    omni.computeXY(-1*con.getStick(2),con.getStick(3),(-1*con.getStick(0))/3.0);
+                    if((con.getButton2(0)==0)&&(con.getButton2(2)==1)){
+                        rote = 0.15;
+                    }else if((con.getButton2(0)==1)&&(con.getButton2(2)==0)){
+                        rote = -0.15;
+                    }else {
+                        rote = 0.0;
+                    }
+                    omni.computeXY(-1*con.getStick(2),con.getStick(3),rote);//(-1*con.getStick(0))/3.0);
                 }else if((con.getStick(0)==0)&&(pidflag==0)) {
                     pid.setProcessValue(R1370.getDeviation(nowAngle));
                     omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
+                }*/
+                if((con.getButton2(0)==0)&&(con.getButton2(2)==1)){
+                    rote = 0.15;
+                }else if((con.getButton2(0)==1)&&(con.getButton2(2)==0)){
+                    rote = -0.15;
+                }else {
+                    rote = 0.0;
                 }
+                omni.computeXY(-1*con.getStick(2),con.getStick(3),rote);
                 pc.printf("Now%lf:D%lf:set%lf:PID%lf   ",R1370.getAngle(),R1370.getDeviation(nowAngle),R1370.getZeroPoint(),-1*pid.compute());
                 for(i = 0; i < 4; i++) {
                     pc.printf("%lf,",omni.getOutput(i));