Soma Takahashi / towelest

mecha

Dependents:   2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue

towelest.cpp@1:c6950f45b03c, 2019-09-04 (annotated)

Committer:
ec30109b
Date:
Wed Sep 04 05:14:18 2019 +0000
Revision:
1:c6950f45b03c
Parent:
0:0797b0e47e6d
Child:
2:623fb33351af
This is new.

Who changed what in which revision?

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