Diff: hayatoBoomerang.cpp

Revision:

1:35e874e10602

Child:

2:aca690545ec9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hayatoBoomerang.cpp	Thu Aug 24 18:16:59 2017 +0900
@@ -0,0 +1,91 @@
+#include "hayatoBoomerang.h"
+
+boomerang::boomerang(Serial *serial_)
+{
+  speedEncoder = new InteruputIn(SPEED_ENCODER);
+  armEncoder= new InteruputIn(ARM_ENCODER);
+
+  armLimit = new InteruptIn(ARM_LIMIT);
+  spiralLimit = new InteruptIn(SPIRAL_LIMIT);
+
+  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);
+
+  armPID.setInputLimits(0,90)
+  speedPID.setOutputLimits(-1.0,1.0);
+
+  speedPID.setBias(0);
+  speedPID.setMode(AUTO);
+  speedPID.setSetPoint(0);
+
+  armPID.setBias(0);
+  armPID.setSetPoint(0);
+  armPID.setMode(AUTO);
+
+  armMotor = new IkarashiMDC(&rscs,0,0,serial_);
+  shootingMotor = new IkarashiMDC(&rscs,0,1,serial_);
+
+  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();
+
+}
+
+void boomerang::shootingEncoderCount()
+{
+  motorRPM = (1.0/rpmCalc.read())*60;
+  rpmCalc.reset();
+  rpmCalc.start();
+}
+
+void boomerang::setTargetMotorSpeed(const int& targetRPM)
+{
+  speedPID.setSetPoint(targetRPM);
+}
+
+void boomerang::setTargetArmAngle(const int& targetArmAngle)
+{
+    armPID.setSetPoint(targetArmAngle);
+}
+
+void booomerang::update()
+{
+  armMotor.setSpeed(armPID.getRealOutput());
+  shootingMotor.setSpeed(speedPID.getRealOutput());
+}
+
+void booomerang::fire()
+{
+  beltMotor.setSpeed(0.4);
+  beltTime.attach(beltStop,0.5);
+  spiral.reload();
+
+}
+
+void boomerang::beltStop()
+{
+  beltMotor.setSpeed(0);
+}
+
+int boomerang::calibrateArm()
+{
+  if(armLimit == true)
+    return 1;
+
+  armLimit.enable_irq();
+  armMotor.setSpeed(0.2);
+  return 0;
+}
+
+void boomerang::endCalibration()
+{
+  armEncoder.reset()
+  armLimit.disable_irq();
+  armPID.setSetPoint(10);
+}