mecha
Dependents: 2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue
Diff: towelest.cpp
- Revision:
- 0:0797b0e47e6d
- Child:
- 1:c6950f45b03c
diff -r 000000000000 -r 0797b0e47e6d towelest.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/towelest.cpp Thu Aug 29 05:14:22 2019 +0000 @@ -0,0 +1,77 @@ +#include "towelest.h" + +towelest::towelest(Serial* RS485): +enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING), +pid(4.0, 0, 0.001, 0.01), +LimitSW1(PB_5), +LimitSW2(PB_4), +pc(USBTX,USBRX,115200) +{ + Motor = new ikarashiMDC*[3]; + Motor[0] = new ikarashiMDC(2,0,SM,RS485); + Motor[1] = new ikarashiMDC(2,1,SM,RS485); + Motor[2] = new ikarashiMDC(2,2,SM,RS485); + + Motor[0]->braking = true; + Motor[1]->braking = true; + Motor[2]->braking = true; + + thread.start(callback(this, &towelest::loop)); + + pid.setInputLimits(-1800, 1800); + pid.setOutputLimits(-0.6,0.6); + pid.setBias(0.0); + pid.setMode(AUTO_MODE); + pid.setSetPoint(0); + +} + +void towelest::open() +{ + if(LimitSW1.read() == 1) { + Motor[0]->setSpeed(0); + Motor[1]->setSpeed(0); + } else if(LimitSW1.read() == 0) { + Motor[0]->setSpeed(-0.2); + Motor[1]->setSpeed(0); + } +} + +void towelest::lift() +{ + if(LimitSW2.read() == 1) { + Motor[0]->setSpeed(0); + Motor[1]->setSpeed(0); + } else if(LimitSW2.read() == 0) { + Motor[0]->setSpeed(0); + Motor[1]->setSpeed(0.8); + } +} + +void towelest::drop() +{ + timer.start(); + time = timer.read(); + if(time <= 3) { + pid.setSetPoint(-800); + } else { + pid.setSetPoint(0); + timer.stop(); + timer.reset(); + } +} + +void towelest::loop() +{ + while(true) { + RotationDistance = (float)enc1.getPulses(); + pid.setProcessValue(RotationDistance); + compute = pid.compute(); + Motor[2]->setSpeed(compute); + Limit1 = LimitSW1.read(); + Limit2 = LimitSW2.read(); + pc.printf("%d %d %f \n\r",Limit1,Limit2,RotationDistance); + + } +} +