Soma Takahashi / towelest

mecha

Dependents:   2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue

towelest.cpp@0:0797b0e47e6d, 2019-08-29 (annotated)

Committer:
ec30109b
Date:
Thu Aug 29 05:14:22 2019 +0000
Revision:
0:0797b0e47e6d
Child:
1:c6950f45b03c
2019NHK_teamA_auto robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 0:0797b0e47e6d 1 #include "towelest.h"
ec30109b 0:0797b0e47e6d 2
ec30109b 0:0797b0e47e6d 3 towelest::towelest(Serial* RS485):
ec30109b 0:0797b0e47e6d 4 enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING),
ec30109b 0:0797b0e47e6d 5 pid(4.0, 0, 0.001, 0.01),
ec30109b 0:0797b0e47e6d 6 LimitSW1(PB_5),
ec30109b 0:0797b0e47e6d 7 LimitSW2(PB_4),
ec30109b 0:0797b0e47e6d 8 pc(USBTX,USBRX,115200)
ec30109b 0:0797b0e47e6d 9 {
ec30109b 0:0797b0e47e6d 10 Motor = new ikarashiMDC*[3];
ec30109b 0:0797b0e47e6d 11 Motor[0] = new ikarashiMDC(2,0,SM,RS485);
ec30109b 0:0797b0e47e6d 12 Motor[1] = new ikarashiMDC(2,1,SM,RS485);
ec30109b 0:0797b0e47e6d 13 Motor[2] = new ikarashiMDC(2,2,SM,RS485);
ec30109b 0:0797b0e47e6d 14
ec30109b 0:0797b0e47e6d 15 Motor[0]->braking = true;
ec30109b 0:0797b0e47e6d 16 Motor[1]->braking = true;
ec30109b 0:0797b0e47e6d 17 Motor[2]->braking = true;
ec30109b 0:0797b0e47e6d 18
ec30109b 0:0797b0e47e6d 19 thread.start(callback(this, &towelest::loop));
ec30109b 0:0797b0e47e6d 20
ec30109b 0:0797b0e47e6d 21 pid.setInputLimits(-1800, 1800);
ec30109b 0:0797b0e47e6d 22 pid.setOutputLimits(-0.6,0.6);
ec30109b 0:0797b0e47e6d 23 pid.setBias(0.0);
ec30109b 0:0797b0e47e6d 24 pid.setMode(AUTO_MODE);
ec30109b 0:0797b0e47e6d 25 pid.setSetPoint(0);
ec30109b 0:0797b0e47e6d 26
ec30109b 0:0797b0e47e6d 27 }
ec30109b 0:0797b0e47e6d 28
ec30109b 0:0797b0e47e6d 29 void towelest::open()
ec30109b 0:0797b0e47e6d 30 {
ec30109b 0:0797b0e47e6d 31 if(LimitSW1.read() == 1) {
ec30109b 0:0797b0e47e6d 32 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 33 Motor[1]->setSpeed(0);
ec30109b 0:0797b0e47e6d 34 } else if(LimitSW1.read() == 0) {
ec30109b 0:0797b0e47e6d 35 Motor[0]->setSpeed(-0.2);
ec30109b 0:0797b0e47e6d 36 Motor[1]->setSpeed(0);
ec30109b 0:0797b0e47e6d 37 }
ec30109b 0:0797b0e47e6d 38 }
ec30109b 0:0797b0e47e6d 39
ec30109b 0:0797b0e47e6d 40 void towelest::lift()
ec30109b 0:0797b0e47e6d 41 {
ec30109b 0:0797b0e47e6d 42 if(LimitSW2.read() == 1) {
ec30109b 0:0797b0e47e6d 43 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 44 Motor[1]->setSpeed(0);
ec30109b 0:0797b0e47e6d 45 } else if(LimitSW2.read() == 0) {
ec30109b 0:0797b0e47e6d 46 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 47 Motor[1]->setSpeed(0.8);
ec30109b 0:0797b0e47e6d 48 }
ec30109b 0:0797b0e47e6d 49 }
ec30109b 0:0797b0e47e6d 50
ec30109b 0:0797b0e47e6d 51 void towelest::drop()
ec30109b 0:0797b0e47e6d 52 {
ec30109b 0:0797b0e47e6d 53 timer.start();
ec30109b 0:0797b0e47e6d 54 time = timer.read();
ec30109b 0:0797b0e47e6d 55 if(time <= 3) {
ec30109b 0:0797b0e47e6d 56 pid.setSetPoint(-800);
ec30109b 0:0797b0e47e6d 57 } else {
ec30109b 0:0797b0e47e6d 58 pid.setSetPoint(0);
ec30109b 0:0797b0e47e6d 59 timer.stop();
ec30109b 0:0797b0e47e6d 60 timer.reset();
ec30109b 0:0797b0e47e6d 61 }
ec30109b 0:0797b0e47e6d 62 }
ec30109b 0:0797b0e47e6d 63
ec30109b 0:0797b0e47e6d 64 void towelest::loop()
ec30109b 0:0797b0e47e6d 65 {
ec30109b 0:0797b0e47e6d 66 while(true) {
ec30109b 0:0797b0e47e6d 67 RotationDistance = (float)enc1.getPulses();
ec30109b 0:0797b0e47e6d 68 pid.setProcessValue(RotationDistance);
ec30109b 0:0797b0e47e6d 69 compute = pid.compute();
ec30109b 0:0797b0e47e6d 70 Motor[2]->setSpeed(compute);
ec30109b 0:0797b0e47e6d 71 Limit1 = LimitSW1.read();
ec30109b 0:0797b0e47e6d 72 Limit2 = LimitSW2.read();
ec30109b 0:0797b0e47e6d 73 pc.printf("%d %d %f \n\r",Limit1,Limit2,RotationDistance);
ec30109b 0:0797b0e47e6d 74
ec30109b 0:0797b0e47e6d 75 }
ec30109b 0:0797b0e47e6d 76 }
ec30109b 0:0797b0e47e6d 77