2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: hayatoBoomerang.cpp
- Revision:
- 8:c47cf4e0230c
- Parent:
- 6:0bee4b2bb400
- Child:
- 9:6540a10a07fb
diff -r baf16d0c14e7 -r c47cf4e0230c hayatoBoomerang.cpp --- a/hayatoBoomerang.cpp Fri Sep 01 11:36:46 2017 +0900 +++ b/hayatoBoomerang.cpp Thu Sep 07 00:05:25 2017 +0000 @@ -4,7 +4,7 @@ boomerang::boomerang(Serial *serial_) { speedEncoder = new InterruptIn(SPEED_ENCODER); - armEncoder= new QEI(D10,D11,NC,624); + armEncoder= new QEI(PB_4,PB_14,NC,624); armLimit = new InterruptIn(NC); //armLimit->mode(PullUp); @@ -14,7 +14,7 @@ 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); + speedPID->setOutputLimits(0,0.5); armPID->setInputLimits(5,80); armPID->setOutputLimits(-1.0,1.0); @@ -31,6 +31,8 @@ shootingMotor = new ikarashiMDC(rscs,0,1,SM,serial_); spiralMotor = new ikarashiMDC(rscs,0,2,SM,serial_); beltMotor = new ikarashiMDC(rscs,0,3,SM,serial_); + spiralMotor->braking=true; + shootingMotor->braking=false; omni = new Omni(4); omniMotor[0] = new ikarashiMDC(rscs,1,0,SM,serial_); @@ -65,9 +67,17 @@ void boomerang::move(const uint8_t& x,const uint8_t& y,const uint8_t& moment) { - omni->computeXY((y-127)/127.0,(x-127)/127.0,moment); + double rotator = 0; +// if(moment < 123 && moment > 130) { + rotator = ((moment-127)/127.0)*0.2; +// } + if((x > 123 && x < 130) && (y > 123 && y < 130)) { + omni->computeXY(0,0,rotator); + } else { + omni->computeXY(((y-127)/127.0)*0.8,((x-127)/127.0)*0.8,rotator); + } for (size_t i = 0; i < 4; i++) { - omniMotor[i]->setSpeed((omni->getOutput(i)-127)/127.0); + omniMotor[i]->setSpeed(omni->getOutput(i)); } }