Diff: hayatoBoomerang.cpp

Revision:

8:c47cf4e0230c

Parent:

6:0bee4b2bb400

Child:

9:6540a10a07fb

--- a/hayatoBoomerang.cpp	Fri Sep 01 11:36:46 2017 +0900
+++ b/hayatoBoomerang.cpp	Thu Sep 07 00:05:25 2017 +0000
@@ -4,7 +4,7 @@
 boomerang::boomerang(Serial *serial_)
 {
   speedEncoder = new InterruptIn(SPEED_ENCODER);
-  armEncoder= new QEI(D10,D11,NC,624);
+  armEncoder= new QEI(PB_4,PB_14,NC,624);
 
   armLimit = new InterruptIn(NC);
   //armLimit->mode(PullUp);
@@ -14,7 +14,7 @@
   speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
   armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
   speedPID->setInputLimits(0.0,9000.0);
-  speedPID->setOutputLimits(0,1.0);
+  speedPID->setOutputLimits(0,0.5);
 
   armPID->setInputLimits(5,80);
   armPID->setOutputLimits(-1.0,1.0);
@@ -31,6 +31,8 @@
   shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
   spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
   beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
+  spiralMotor->braking=true;
+  shootingMotor->braking=false;
 
   omni = new Omni(4);
   omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
@@ -65,9 +67,17 @@
 
 void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment)
 {
-    omni->computeXY((y-127)/127.0,(x-127)/127.0,moment);
+    double rotator = 0;
+//    if(moment < 123 && moment > 130) {
+        rotator = ((moment-127)/127.0)*0.2;
+//    }
+    if((x > 123 && x < 130) && (y > 123 && y < 130)) {
+        omni->computeXY(0,0,rotator);
+    } else {
+        omni->computeXY(((y-127)/127.0)*0.8,((x-127)/127.0)*0.8,rotator);
+    }
     for (size_t i = 0; i < 4; i++) {
-      omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0);
+      omniMotor[i]->setSpeed(omni->getOutput(i));
     }
 }