mecha
Dependents: 2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue
Diff: towelest.cpp
- Revision:
- 1:c6950f45b03c
- Parent:
- 0:0797b0e47e6d
- Child:
- 2:623fb33351af
diff -r 0797b0e47e6d -r c6950f45b03c towelest.cpp --- a/towelest.cpp Thu Aug 29 05:14:22 2019 +0000 +++ b/towelest.cpp Wed Sep 04 05:14:18 2019 +0000 @@ -1,16 +1,15 @@ #include "towelest.h" -towelest::towelest(Serial* RS485): +towelest::towelest(): +serial(mdTX,mdRX,115200), 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) +LimitSW2(PB_4) { - 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] = new ikarashiMDC(2,0,SM,&serial); + Motor[1] = new ikarashiMDC(2,1,SM,&serial); + Motor[2] = new ikarashiMDC(2,2,SM,&serial); Motor[0]->braking = true; Motor[1]->braking = true; @@ -28,10 +27,10 @@ void towelest::open() { - if(LimitSW1.read() == 1) { + if(LimitSW1.read() == 1){ Motor[0]->setSpeed(0); Motor[1]->setSpeed(0); - } else if(LimitSW1.read() == 0) { + }else if(LimitSW1.read() == 0){ Motor[0]->setSpeed(-0.2); Motor[1]->setSpeed(0); } @@ -52,7 +51,7 @@ { timer.start(); time = timer.read(); - if(time <= 3) { + if(time <= 5) { pid.setSetPoint(-800); } else { pid.setSetPoint(0); @@ -63,15 +62,14 @@ void towelest::loop() { - while(true) { + 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); - + //pc.printf("%d %d %f \n\r",Limit1,Limit2,RotationDistance); } }