Diff: hayatoBoomerang.cpp

Revision:

12:9d21189a8eb0

Parent:

11:68defd561031

Child:

13:d01356a29b93

--- a/hayatoBoomerang.cpp	Fri Sep 01 14:04:21 2017 +0900
+++ b/hayatoBoomerang.cpp	Wed Sep 13 22:58:42 2017 +0700
@@ -4,23 +4,30 @@
 boomerang::boomerang(Serial *serial_)
 {
   speedEncoder = new InterruptIn(SPEED_ENCODER);
+  armEncoder= new QEI(PB_15,PB_4,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);
 
-  armValve[0] = new DigitalOut(D13);
-  armValve[1] = new DigitalOut(D13);
+  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_);
@@ -35,8 +42,7 @@
   omniMotor[2]->braking=true;
   omniMotor[3]->braking=true;
 
-  armValve[0]->write(false);
-  armValve[1]->write(false);
+  armMotor->setSpeed(0);
   shootingMotor->setSpeed(0);
   spiralMotor->setSpeed(0);
   beltMotor->setSpeed(0);
@@ -45,10 +51,13 @@
 
   motorRPM = 0;
 
-  omni->setWheelRadian(radians(45),radians(45+90),radians(45+180),radians(45+270));
+  omni->setWheelRadian(radians(51.675),radians(76.65+51.675),radians(76.65+51.675+103.35),radians(360-51.675));
 
   speedEncoder->rise(this,&boomerang::shootingEncoderCount);
   speedEncoder->enable_irq();
+  armLimit->fall(this,&boomerang::endCalibration);
+  armLimit->disable_irq();
+
   firing = false;
 
   boomerang::update();
@@ -74,10 +83,18 @@
   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);
 }
@@ -102,8 +119,21 @@
 
 }
 
-void boomerang::sword()
+int boomerang::calibrateArm()
 {
-  armValve[0]->write(!armValve[0]->read());
-  armValve[1]->write(!armValve[0]->read());
+  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);
+}