Diff: hayatoBoomerang.cpp

Revision:

4:c2b09fa31492

Parent:

2:aca690545ec9

Child:

6:0bee4b2bb400

--- a/hayatoBoomerang.cpp	Thu Aug 24 20:51:12 2017 +0900
+++ b/hayatoBoomerang.cpp	Tue Aug 29 17:00:05 2017 +0900
@@ -6,7 +6,8 @@
   speedEncoder = new InterruptIn(SPEED_ENCODER);
   armEncoder= new QEI(NC,NC,NC,720);
 
-  armLimit = new InterruptIn(ARM_LIMIT);
+  armLimit = new InterruptIn(NC);
+  //armLimit->mode(PullUp);
 
   rscs = new DigitalOut(NC);
 
@@ -31,13 +32,18 @@
   spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
   beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
 
+  armMotor->setSpeed(0);
+  shootingMotor->setSpeed(0);
+  spiralMotor->setSpeed(0);
+  beltMotor->setSpeed(0);
+
   spiral = new Spiral(spiralMotor);
 
   motorRPM = 0;
 
   speedEncoder->rise(this,&boomerang::shootingEncoderCount);
   speedEncoder->enable_irq();
-  armLimit->rise(this,&boomerang::endCalibration);
+  armLimit->fall(this,&boomerang::endCalibration);
   armLimit->disable_irq();
 
 }
@@ -61,22 +67,31 @@
 
 void boomerang::update()
 {
-  armMotor->setSpeed(armPID->compute());
-  shootingMotor->setSpeed(speedPID->compute());
+  if(!calibrating)
+    // armMotor->setSpeed(armPID->compute());
+  // shootingMotor->setSpeed(speedPID->compute());
+  beltMotor->setSpeed(beltSpeed);
 }
 
 bool boomerang::fire()
 {
-  beltMotor->setSpeed(0.4);
-  beltTime.attach(this,&boomerang::beltStop,0.5);
+  if(firing)
+    return false;
+  firing = true;
+  beltSpeed=0.4;
+  beltTime.attach(callback(this,&boomerang::beltStop),0.5);
   spiral->rotate();
+  boomerang::update();
 
   return true;
 }
 
 void boomerang::beltStop()
 {
-  beltMotor->setSpeed(0);
+  beltSpeed = 0;
+  firing = false;
+  boomerang::update();
+
 }
 
 int boomerang::calibrateArm()
@@ -84,6 +99,7 @@
   if(armLimit->read() == true)
     return 1;
 
+  calibrating = true;
   armLimit->enable_irq();
   armMotor->setSpeed(0.2);
   return 0;
@@ -91,6 +107,7 @@
 
 void boomerang::endCalibration()
 {
+  calibrating = false;
   armEncoder->reset();
   armLimit->disable_irq();
   armPID->setSetPoint(10);