Diff: main.cpp

Revision:

13:d01356a29b93

Parent:

12:9d21189a8eb0

Child:

14:5cc44bec9cfc

--- a/main.cpp	Wed Sep 13 22:58:42 2017 +0700
+++ b/main.cpp	Thu Oct 05 16:04:46 2017 +0900
@@ -2,6 +2,7 @@
 #include "hayatoBoomerang.h"
 #include "recieveController.h"
 #include "omni.h"
+#include "gakugaku.h"
 
 
 Serial rs485(PC_10,PC_11,38400);
@@ -9,11 +10,8 @@
 PwmOut led(LED1);
 DigitalIn button(BUTTON1);
 boomerang boom(&rs485);
-Thread commThread;
+gakugaku fishingPole(&rs485);
 recieveController con(PC_12,PD_2,198);
-// recieveController con(PA_0,PA_1,198);
-// Serial con(PA_0,PA_1,115200);
-
 
 void init()
 {
@@ -25,37 +23,45 @@
   boom.setTargetArmAngle(40);
 }
 
-void updater()
-{
-  boom.update();
-}
-
 int main() {
   init();
-  int angle = 5,turnRate = 127;
-  bool prevAnglechange = false;
+  int angle = 10,turnRate = 127;
+  double angling;
+  bool prevAnglechange = false,prevSwing=false;
   char data[20],*data2,*data3;
   // commThread.start(&comm);
   // ticker.attach(&updater,0.01);
   while(true)
   {
-    if((con.buttons[0] >>7) )
+    if((con.buttons[0] >>7)%2 )
       boom.fire();
-    if(con.buttons[0] >>8)
-      boom.setTargetMotorSpeed(8000);
+
+    if((con.buttons[0] >>8)%2)
+      boom.setTargetMotorSpeed(13000);
     else
       boom.setTargetMotorSpeed(0);
 
+    if((con.buttons[0]>>6)%2)
+      angle = 10;
+
+    if((con.buttons[0]>>13)%2 )
+    {
+      if(!prevSwing)
+        fishingPole.toggleSwing();
+      prevSwing = true;
+    }else{
+      prevSwing = false;
+    }
     if(con.buttons[3] < 117 )
     {
       if(!prevAnglechange)
-        angle += 1;
+        angle += 5;
       prevAnglechange =true;
     }else
     if(con.buttons[3] > 137)
     {
       if(!prevAnglechange)
-        angle -= 1;
+        angle -= 5;
       prevAnglechange =true;
     }else{
       prevAnglechange =false;
@@ -67,19 +73,32 @@
       angle = 80;
     }
 
-    if((con.buttons[0] >> 9))
-      turnRate = 227;
-    else if((con.buttons[0] >> 10))
-      turnRate = 27;
+    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;
     else
       turnRate = 127;
 
+    if((con.buttons[0]>>14)%2)
+      angling = 0.4;
+    else if((con.buttons[0]>>12)%2)
+      angling = -0.4;
+    else
+      angling = 0;
+    fishingPole.setAnglingSpeed(angling);
+
+    if((con.buttons[0]>>15)%2)
+      fishingPole.Deploy();
+
+    fishingPole.update();
+
 
     boom.setTargetArmAngle(angle);
     led = boom.beltSpeed;
     boom.move(con.buttons[1],con.buttons[2],turnRate);
     boom.update();
-    printf("SPPED:%d\n",con.buttons[0]);
+    printf("PID input:%d  PID result %d\n",angle,con.buttons[0]);
     wait_ms(20);
       /* code */
      con.update();