Diff: hayatoBoomerang.cpp

Revision:

11:68defd561031

Parent:

6:0bee4b2bb400

Child:

12:9d21189a8eb0

diff -r baf16d0c14e7 -r 68defd561031 hayatoBoomerang.cpp
--- a/hayatoBoomerang.cpp	Fri Sep 01 11:36:46 2017 +0900
+++ b/hayatoBoomerang.cpp	Fri Sep 01 14:04:21 2017 +0900
@@ -4,30 +4,23 @@
 boomerang::boomerang(Serial *serial_)
 {
   speedEncoder = new InterruptIn(SPEED_ENCODER);
-  armEncoder= new QEI(D10,D11,NC,624);
 
-  armLimit = new InterruptIn(NC);
   //armLimit->mode(PullUp);
 
   rscs = new DigitalOut(NC);
 
   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);
 
-  armPID->setInputLimits(5,80);
-  armPID->setOutputLimits(-1.0,1.0);
 
   speedPID->setBias(0);
   speedPID->setMode(AUTO_MODE);
   speedPID->setSetPoint(0);
 
-  armPID->setBias(0);
-  armPID->setSetPoint(0);
-  armPID->setMode(AUTO_MODE);
 
-  armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
+  armValve[0] = new DigitalOut(D13);
+  armValve[1] = new DigitalOut(D13);
   shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
   spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
   beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
@@ -42,7 +35,8 @@
   omniMotor[2]->braking=true;
   omniMotor[3]->braking=true;
 
-  armMotor->setSpeed(0);
+  armValve[0]->write(false);
+  armValve[1]->write(false);
   shootingMotor->setSpeed(0);
   spiralMotor->setSpeed(0);
   beltMotor->setSpeed(0);
@@ -51,13 +45,10 @@
 
   motorRPM = 0;
 
-  omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
+  omni->setWheelRadian(radians(45),radians(45+90),radians(45+180),radians(45+270));
 
   speedEncoder->rise(this,&boomerang::shootingEncoderCount);
   speedEncoder->enable_irq();
-  armLimit->fall(this,&boomerang::endCalibration);
-  armLimit->disable_irq();
-
   firing = false;
 
   boomerang::update();
@@ -83,18 +74,10 @@
   speedPID->setSetPoint(targetRPM);
 }
 
-void boomerang::setTargetArmAngle(const int& targetArmAngle)
-{
-    armPID->setSetPoint(targetArmAngle);
-}
 
 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());
   beltMotor->setSpeed(beltSpeed);
 }
@@ -119,21 +102,8 @@
 
 }
 
-int boomerang::calibrateArm()
+void boomerang::sword()
 {
-  if(armLimit->read() == true)
-    return 1;
-
-  calibrating = true;
-  armLimit->enable_irq();
-  armMotor->setSpeed(0.2);
-  return 0;
+  armValve[0]->write(!armValve[0]->read());
+  armValve[1]->write(!armValve[0]->read());
 }
-
-void boomerang::endCalibration()
-{
-  calibrating = false;
-  armEncoder->reset();
-  armLimit->disable_irq();
-  armPID->setSetPoint(10);
-}