Diff: hayatoBoomerang.cpp

Revision:

6:0bee4b2bb400

Parent:

4:c2b09fa31492

Child:

8:c47cf4e0230c

Child:

11:68defd561031

--- a/hayatoBoomerang.cpp	Wed Aug 30 03:44:00 2017 +0900
+++ b/hayatoBoomerang.cpp	Fri Sep 01 10:42:27 2017 +0900
@@ -4,7 +4,7 @@
 boomerang::boomerang(Serial *serial_)
 {
   speedEncoder = new InterruptIn(SPEED_ENCODER);
-  armEncoder= new QEI(NC,NC,NC,720);
+  armEncoder= new QEI(D10,D11,NC,624);
 
   armLimit = new InterruptIn(NC);
   //armLimit->mode(PullUp);
@@ -13,11 +13,11 @@
 
   speedPID = new PID(SPEED_P,SPEED_I,SPEED_D,RATE);
   armPID = new PID(ARM_P,ARM_I,ARM_D,RATE);
-  speedPID->setInputLimits(0.0,6000.0);
+  speedPID->setInputLimits(0.0,9000.0);
   speedPID->setOutputLimits(0,1.0);
 
-  armPID->setInputLimits(0,90);
-  speedPID->setOutputLimits(-1.0,1.0);
+  armPID->setInputLimits(5,80);
+  armPID->setOutputLimits(-1.0,1.0);
 
   speedPID->setBias(0);
   speedPID->setMode(AUTO_MODE);
@@ -32,6 +32,16 @@
   spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
   beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
 
+  omni = new Omni(4);
+  omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_);
+  omniMotor[1] = new ikarashiMDC(rscs,1,1,SM,serial_);
+  omniMotor[2] = new ikarashiMDC(rscs,1,2,SM,serial_);
+  omniMotor[3] = new ikarashiMDC(rscs,1,3,SM,serial_);
+  omniMotor[0]->braking=true;
+  omniMotor[1]->braking=true;
+  omniMotor[2]->braking=true;
+  omniMotor[3]->braking=true;
+
   armMotor->setSpeed(0);
   shootingMotor->setSpeed(0);
   spiralMotor->setSpeed(0);
@@ -41,11 +51,24 @@
 
   motorRPM = 0;
 
+  omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
+
   speedEncoder->rise(this,&boomerang::shootingEncoderCount);
   speedEncoder->enable_irq();
   armLimit->fall(this,&boomerang::endCalibration);
   armLimit->disable_irq();
 
+  firing = false;
+
+  boomerang::update();
+}
+
+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);
+    for (size_t i = 0; i < 4; i++) {
+      omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0);
+    }
 }
 
 void boomerang::shootingEncoderCount()
@@ -67,9 +90,12 @@
 
 void boomerang::update()
 {
+  armAngle = armEncoder->getPulses()/800.0*85.51;
+  armPID->setProcessValue(armAngle);
+  speedPID->setProcessValue(motorRPM);
   if(!calibrating)
-    // armMotor->setSpeed(armPID->compute());
-  // shootingMotor->setSpeed(speedPID->compute());
+    armMotor->setSpeed(armPID->compute());
+  shootingMotor->setSpeed(speedPID->compute());
   beltMotor->setSpeed(beltSpeed);
 }
 
@@ -79,10 +105,9 @@
     return false;
   firing = true;
   beltSpeed=0.4;
-  beltTime.attach(callback(this,&boomerang::beltStop),0.5);
+  beltTime.attach(callback(this,&boomerang::beltStop),1);
   spiral->rotate();
   boomerang::update();
-
   return true;
 }