Diff: hayatoBoomerang.cpp

Revision:

2:aca690545ec9

Parent:

1:35e874e10602

Child:

4:c2b09fa31492

--- a/hayatoBoomerang.cpp	Thu Aug 24 18:16:59 2017 +0900
+++ b/hayatoBoomerang.cpp	Thu Aug 24 20:47:56 2017 +0900
@@ -1,39 +1,44 @@
 #include "hayatoBoomerang.h"
+#include "pinConfig.h"
 
 boomerang::boomerang(Serial *serial_)
 {
-  speedEncoder = new InteruputIn(SPEED_ENCODER);
-  armEncoder= new InteruputIn(ARM_ENCODER);
+  speedEncoder = new InterruptIn(SPEED_ENCODER);
+  armEncoder= new QEI(NC,NC,NC,720);
 
-  armLimit = new InteruptIn(ARM_LIMIT);
-  spiralLimit = new InteruptIn(SPIRAL_LIMIT);
+  armLimit = new InterruptIn(ARM_LIMIT);
+
+  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,6000.0);
-  speedPID.setOutputLimits(0,1.0);
+  speedPID->setInputLimits(0.0,6000.0);
+  speedPID->setOutputLimits(0,1.0);
 
-  armPID.setInputLimits(0,90)
-  speedPID.setOutputLimits(-1.0,1.0);
+  armPID->setInputLimits(0,90);
+  speedPID->setOutputLimits(-1.0,1.0);
+
+  speedPID->setBias(0);
+  speedPID->setMode(AUTO_MODE);
+  speedPID->setSetPoint(0);
 
-  speedPID.setBias(0);
-  speedPID.setMode(AUTO);
-  speedPID.setSetPoint(0);
+  armPID->setBias(0);
+  armPID->setSetPoint(0);
+  armPID->setMode(AUTO_MODE);
 
-  armPID.setBias(0);
-  armPID.setSetPoint(0);
-  armPID.setMode(AUTO);
+  armMotor = new ikarashiMDC(rscs,0,0,SM,serial_);
+  shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_);
+  spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_);
+  beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_);
 
-  armMotor = new IkarashiMDC(&rscs,0,0,serial_);
-  shootingMotor = new IkarashiMDC(&rscs,0,1,serial_);
+  spiral = new Spiral(spiralMotor);
 
   motorRPM = 0;
 
-  armEncoder = new QEI(ARM_ENCODER_A,ARM_ENCODER_B,NC,720,X4_ENCODING);
-  speedEncoder.rise(&shootingEncoderCount);
-  speedEncoder.enable_irq();
-  armLimit.rise(&endCalibration);
-  armLimit.disable_irq();
+  speedEncoder->rise(this,&boomerang::shootingEncoderCount);
+  speedEncoder->enable_irq();
+  armLimit->rise(this,&boomerang::endCalibration);
+  armLimit->disable_irq();
 
 }
 
@@ -46,46 +51,47 @@
 
 void boomerang::setTargetMotorSpeed(const int& targetRPM)
 {
-  speedPID.setSetPoint(targetRPM);
+  speedPID->setSetPoint(targetRPM);
 }
 
 void boomerang::setTargetArmAngle(const int& targetArmAngle)
 {
-    armPID.setSetPoint(targetArmAngle);
+    armPID->setSetPoint(targetArmAngle);
 }
 
-void booomerang::update()
+void boomerang::update()
 {
-  armMotor.setSpeed(armPID.getRealOutput());
-  shootingMotor.setSpeed(speedPID.getRealOutput());
+  armMotor->setSpeed(armPID->compute());
+  shootingMotor->setSpeed(speedPID->compute());
 }
 
-void booomerang::fire()
+bool boomerang::fire()
 {
-  beltMotor.setSpeed(0.4);
-  beltTime.attach(beltStop,0.5);
-  spiral.reload();
+  beltMotor->setSpeed(0.4);
+  beltTime.attach(this,&boomerang::beltStop,0.5);
+  spiral->rotate();
 
+  return true;
 }
 
 void boomerang::beltStop()
 {
-  beltMotor.setSpeed(0);
+  beltMotor->setSpeed(0);
 }
 
 int boomerang::calibrateArm()
 {
-  if(armLimit == true)
+  if(armLimit->read() == true)
     return 1;
 
-  armLimit.enable_irq();
-  armMotor.setSpeed(0.2);
+  armLimit->enable_irq();
+  armMotor->setSpeed(0.2);
   return 0;
 }
 
 void boomerang::endCalibration()
 {
-  armEncoder.reset()
-  armLimit.disable_irq();
-  armPID.setSetPoint(10);
+  armEncoder->reset();
+  armLimit->disable_irq();
+  armPID->setSetPoint(10);
 }