Diff: hayatoBoomerang.cpp

Revision:

17:311aed3cad15

Parent:

16:5f205692b14d

Child:

18:4b629221c215

--- a/hayatoBoomerang.cpp	Fri Oct 06 21:59:16 2017 +0900
+++ b/hayatoBoomerang.cpp	Sun Oct 15 13:42:12 2017 +0900
@@ -1,8 +1,9 @@
 #include "hayatoBoomerang.h"
 #include "pinConfig.h"
 
-boomerang::boomerang(Serial *serial_)
+boomerang::boomerang(Serial *serial_,Serial *serial2_)
 {
+  spiralSerial = serial2_;
   speedEncoder = new InterruptIn(SPEED_ENCODER);
   armEncoder= new QEI(PB_15,PB_4,NC,624);
 
@@ -29,7 +30,7 @@
 
   armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
   shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
-  spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
+  spiralMotor = new ikarashiMDC(rscs,0,0,SM,spiralSerial);
   beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
 
   omni = new Omni(4);
@@ -43,6 +44,10 @@
   omniMotor[2]->braking=true;
   omniMotor[3]->braking=true;
 
+  shootingMotor->braking=false;
+
+  spiralMotor->braking=true;
+
   armMotor->setSpeed(0);
   shootingMotor->setSpeed(0);
   spiralMotor->setSpeed(0);
@@ -60,12 +65,8 @@
 
   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)
@@ -85,6 +86,11 @@
 
 void boomerang::setTargetMotorSpeed(const int& targetRPM)
 {
+  if(targetRPM == 0){
+    speedPID->reset();
+    speedPID->setSetPoint(0);
+    return;
+  }
   speedPID->setSetPoint(targetRPM);
 }
 
@@ -98,48 +104,16 @@
   armAngle = armEncoder->getPulses()/800.0*85.51;
   armPID->setProcessValue(armAngle);
   speedPID->setProcessValue(motorRPM);
-  if(!calibrating)
-    armMotor->setSpeed(armPID->compute());
+  armMotor->setSpeed(armPID->compute());
   shootingMotor->setSpeed(speedPID->compute());
-  beltMotor->setSpeed(beltSpeed);
-
+  beltMotor->setSpeed(spiral->beltSpeed);
+  spiralMotor->setSpeed(spiral->spiralSpeed);
 }
 
 bool boomerang::fire()
 {
-  if(firing)
+  if(spiral->firing)
     return false;
-  firing = true;
-  beltSpeed=0.4;
-  beltTime.attach(callback(this,&boomerang::beltStop),1);
   spiral->rotate();
-  boomerang::update();
   return true;
 }
-
-void boomerang::beltStop()
-{
-  beltSpeed = 0;
-  firing = false;
-  boomerang::update();
-
-}
-
-int boomerang::calibrateArm()
-{
-  if(armLimit->read() == true)
-    return 1;
-
-  calibrating = true;
-  armLimit->enable_irq();
-  armMotor->setSpeed(0.2);
-  return 0;
-}
-
-void boomerang::endCalibration()
-{
-  calibrating = false;
-  armEncoder->reset();
-  armLimit->disable_irq();
-  armPID->setSetPoint(10);
-}