Diff: Spiral.cpp

Revision:

20:d052a0679309

Parent:

18:4b629221c215

Child:

23:ba636c80ed1b

diff -r 31784928ec2d -r d052a0679309 Spiral.cpp
--- a/Spiral.cpp	Sun Nov 05 09:35:20 2017 +0900
+++ b/Spiral.cpp	Mon Nov 06 18:54:45 2017 +0900
@@ -2,15 +2,26 @@
 
 Spiral::Spiral(ikarashiMDC *spiralMotor_)
 {
-  beltSpeed = 0;
+  beltSpeed = 0.0;
   spiralSpeed = 0;
   spiralMotor = spiralMotor_;
   spiralLimit = new InterruptIn(PB_12);
   spiralLimit->mode(PullUp);
   spiralLimit->rise(this,&Spiral::stopRotation);
+  beltPID = new PID(60,1e-47,0.00005,0.01);
   rotating = false;
   firing = false;
   spiralLimit->enable_irq();
+  
+  beltEncoder = new QEI(PA_11,PA_7,NC,720);
+
+  beltPID->setOutputLimits(-1.0,1.0);
+  beltPID->setInputLimits(-10000,10000);
+  beltPID->setBias(0);
+  beltPID->setMode(AUTO_MODE);
+  beltPID->setSetPoint(0);
+  beltPos = 0;
+
 }
 
 int Spiral::rotate()
@@ -29,16 +40,20 @@
   if (!rotating)return;
   spiralSpeed=0;
   spiralMotor->setSpeed(0);
-  rotating = false;
   beltTime.attach(callback(this,&Spiral::beltStart),0.1);
 }
 
 void Spiral::beltStart()
 {
-  beltSpeed=-1;
-  beltTime.attach(callback(this,&Spiral::beltStop),0.1);
+  beltEncoder->reset();
+  beltPos =200;
+  beltPID->setSetPoint(beltPos);
+  rotating = false;
 }
-void Spiral::beltStop(){
-  beltSpeed=0;
-  firing = false;
+void Spiral::update()
+{
+  beltPID->setProcessValue(beltEncoder->getPulses()/720.0*5*30);
+  beltPID->setSetPoint(beltPos);
+  beltSpeed = -beltPID->compute();
+  if((beltEncoder->getPulses()/720.0*5*30 >beltPos-10) && rotating == false ) firing = false;
 }