Diff: main.cpp

Revision:

18:4b629221c215

Parent:

17:311aed3cad15

Child:

20:d052a0679309

Child:

24:050d78bb5b69

--- a/main.cpp	Sun Oct 15 13:42:12 2017 +0900
+++ b/main.cpp	Thu Nov 02 20:40:13 2017 +0900
@@ -15,9 +15,13 @@
 gakugaku fishingPole(&spiral);
 Ticker debugesp,ticker;
 recieveController con(PC_12,PD_2,198);
+BusIn modeChange(PA_4,PB_0,PC_2,PC_1);
+BusOut modeStatus(NC,NC,NC,NC);
+BusOut angleState(NC,NC,NC);
 
 double angle = 10;
 
+
 void update()
 {
   boom.update();
@@ -39,34 +43,44 @@
 
 int main() {
   init();
-  int turnRate = 127;
+  double turnRate = 0;
+  double flywheelspeed;
   double angling;
   bool prevAnglechange = false,prevSwing=false;
-  char data[20],*data2,*data3;
+    char data[20],*data2,*data3;
+  modeChange.mode(PullUp);
+  if((15-modeChange.read())%3 == 0)
+    flywheelspeed =13000;
+  else if(((15-modeChange.read())%3) == 1)
+    flywheelspeed =13000*3.0/4.0;
+  else if(((15-modeChange.read())%3) == 2)
+    flywheelspeed = 13000*2.0/3.0;
   //debugesp.attach(&debugout,3);
   // commThread.start(&comm);
   while(true)
   {
-    if((con.buttons[0] >>7)%2 )
+    modeStatus = 15-modeChange.read();
+    if((con.buttons[0] >>9)%2 )
     {
       boom.fire();
     }
 
-    if((con.buttons[0] >>8)%2)
-      boom.setTargetMotorSpeed(13000);
+    if((con.buttons[0] >>10)%2)
+      boom.setTargetMotorSpeed(flywheelspeed);
+
     else
       boom.setTargetMotorSpeed(0);
 
 
-    if((con.buttons[0]>>3)%2) //square
+    if((con.buttons[0]>>12)%2) //square
       angle = 80;
-    if((con.buttons[0]>>4)%2) //cross
+    if((con.buttons[0]>>14)%2) //cross
       angle = 45;
-    if((con.buttons[0]>>5)%2) //circle
+    if((con.buttons[0]>>13)%2) //circle
       angle = 25;
-    if((con.buttons[0]>>6)%2) //triangle
+    if((con.buttons[0]>>11)%2) //triangle
       angle = 10;
-    if((con.buttons[0]>>13)%2 )
+    if((con.buttons[0]>>2)%2 )
     {
       if(!prevSwing)
         fishingPole.toggleSwing();
@@ -74,13 +88,13 @@
     }else{
       prevSwing = false;
     }
-    if(con.buttons[3] < 117 )
+    if(con.buttons[4] < 117 )
     {
       if(!prevAnglechange)
         angle += 2.5;
       prevAnglechange =true;
     }else
-    if(con.buttons[3] > 137)
+    if(con.buttons[4] > 137)
     {
       if(!prevAnglechange)
         angle -= 2.5;
@@ -95,32 +109,31 @@
       angle = 90;
     }
 
-    if((con.buttons[0] >> 9)%2)
-      turnRate = 127-127*.2; //turn at .2 power
-    else if((con.buttons[0] >> 10)%2)
-      turnRate = 127+127*.2;
+    if((con.buttons[3] >50))
+      turnRate = con.buttons[3]/500.0;
+    else if((con.buttons[6]/4080 > 50))
+      turnRate = -(con.buttons[6]/4080)/500.0;
     else
-      turnRate = 127;
+      turnRate = 0;
 
-    if((con.buttons[0]>>14)%2)
+    if((con.buttons[0]%2))
       angling = -0.4;
-    else if((con.buttons[0]>>12)%2)
-      angling = 0.4;
+    else if((con.buttons[0]>>1)%2)
+      angling = 0.2;
     else
       angling = 0;
     fishingPole.setAnglingSpeed(angling);
 
-    if((con.buttons[0]>>15)%2)
+    if((con.buttons[0]>>8)%2)
       fishingPole.Deploy();
 
-
-
     update();
 
     boom.setTargetArmAngle(angle);
+    angleState = int(angle/90.0*7);
     led = boom.beltSpeed;
     boom.move(con.buttons[1],con.buttons[2],turnRate);
-    serial.printf("controller:%f\n\r",boom.motorRPM);
+    serial.printf("controller:%d\n\r",con.buttons[0]);
      con.update();
 
   }