Soma Takahashi / towelest

mecha

Dependents:   2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue

towelest.cpp@2:623fb33351af, 2019-09-13 (annotated)

Committer:
ec30109b
Date:
Fri Sep 13 04:38:47 2019 +0000
Revision:
2:623fb33351af
Parent:
1:c6950f45b03c
Child:
3:63bb3e19c7eb
add setPulse

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 2:623fb33351af 4 serial(mdTX,mdRX,115200),
ec30109b 2:623fb33351af 5 pid(4.0, 0, 0.001, 0.01),
ec30109b 2:623fb33351af 6 LimitSW1(schmitt_trigger_0),
ec30109b 2:623fb33351af 7 LimitSW2(schmitt_trigger_1)
ec30109b 0:0797b0e47e6d 8 {
ec30109b 1:c6950f45b03c 9 Motor[0] = new ikarashiMDC(2,0,SM,&serial);
ec30109b 2:623fb33351af 10 Motor[1] = new ikarashiMDC(2,2,SM,&serial);
ec30109b 2:623fb33351af 11 Motor[2] = new ikarashiMDC(2,1,SM,&serial);
ec30109b 0:0797b0e47e6d 12
ec30109b 0:0797b0e47e6d 13 Motor[0]->braking = true;
ec30109b 0:0797b0e47e6d 14 Motor[1]->braking = true;
ec30109b 0:0797b0e47e6d 15 Motor[2]->braking = true;
ec30109b 0:0797b0e47e6d 16
ec30109b 0:0797b0e47e6d 17 thread.start(callback(this, &towelest::loop));
ec30109b 0:0797b0e47e6d 18
ec30109b 0:0797b0e47e6d 19 pid.setInputLimits(-1800, 1800);
ec30109b 0:0797b0e47e6d 20 pid.setOutputLimits(-0.6,0.6);
ec30109b 0:0797b0e47e6d 21 pid.setBias(0.0);
ec30109b 0:0797b0e47e6d 22 pid.setMode(AUTO_MODE);
ec30109b 0:0797b0e47e6d 23 pid.setSetPoint(0);
ec30109b 0:0797b0e47e6d 24
ec30109b 0:0797b0e47e6d 25 }
ec30109b 0:0797b0e47e6d 26
ec30109b 0:0797b0e47e6d 27 void towelest::open()
ec30109b 0:0797b0e47e6d 28 {
ec30109b 2:623fb33351af 29 if(LimitSW1.read() == 1) {
ec30109b 0:0797b0e47e6d 30 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 31 Motor[1]->setSpeed(0);
ec30109b 2:623fb33351af 32 } else if(LimitSW1.read() == 0) {
ec30109b 0:0797b0e47e6d 33 Motor[0]->setSpeed(-0.2);
ec30109b 0:0797b0e47e6d 34 Motor[1]->setSpeed(0);
ec30109b 0:0797b0e47e6d 35 }
ec30109b 0:0797b0e47e6d 36 }
ec30109b 2:623fb33351af 37 void towelest::close()
ec30109b 2:623fb33351af 38 {
ec30109b 2:623fb33351af 39 Motor[0]->setSpeed(0.2);
ec30109b 2:623fb33351af 40 Motor[1]->setSpeed(0);
ec30109b 2:623fb33351af 41 }
ec30109b 2:623fb33351af 42
ec30109b 0:0797b0e47e6d 43 void towelest::lift()
ec30109b 0:0797b0e47e6d 44 {
ec30109b 0:0797b0e47e6d 45 if(LimitSW2.read() == 1) {
ec30109b 0:0797b0e47e6d 46 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 47 Motor[1]->setSpeed(0);
ec30109b 0:0797b0e47e6d 48 } else if(LimitSW2.read() == 0) {
ec30109b 0:0797b0e47e6d 49 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 50 Motor[1]->setSpeed(0.8);
ec30109b 0:0797b0e47e6d 51 }
ec30109b 0:0797b0e47e6d 52 }
ec30109b 0:0797b0e47e6d 53
ec30109b 0:0797b0e47e6d 54 void towelest::drop()
ec30109b 0:0797b0e47e6d 55 {
ec30109b 0:0797b0e47e6d 56 timer.start();
ec30109b 0:0797b0e47e6d 57 time = timer.read();
ec30109b 1:c6950f45b03c 58 if(time <= 5) {
ec30109b 0:0797b0e47e6d 59 pid.setSetPoint(-800);
ec30109b 0:0797b0e47e6d 60 } else {
ec30109b 0:0797b0e47e6d 61 pid.setSetPoint(0);
ec30109b 0:0797b0e47e6d 62 timer.stop();
ec30109b 0:0797b0e47e6d 63 timer.reset();
ec30109b 0:0797b0e47e6d 64 }
ec30109b 0:0797b0e47e6d 65 }
ec30109b 0:0797b0e47e6d 66
ec30109b 2:623fb33351af 67 void towelest::setPulse(int Loli)
ec30109b 2:623fb33351af 68 {
ec30109b 2:623fb33351af 69 loli = Loli;
ec30109b 2:623fb33351af 70 }
ec30109b 2:623fb33351af 71
ec30109b 0:0797b0e47e6d 72 void towelest::loop()
ec30109b 0:0797b0e47e6d 73 {
ec30109b 2:623fb33351af 74 while(true) {
ec30109b 2:623fb33351af 75 pid.setProcessValue(loli);
ec30109b 0:0797b0e47e6d 76 compute = pid.compute();
ec30109b 0:0797b0e47e6d 77 Motor[2]->setSpeed(compute);
ec30109b 0:0797b0e47e6d 78 Limit1 = LimitSW1.read();
ec30109b 0:0797b0e47e6d 79 Limit2 = LimitSW2.read();
ec30109b 1:c6950f45b03c 80 //pc.printf("%d %d %f \n\r",Limit1,Limit2,RotationDistance);
ec30109b 0:0797b0e47e6d 81 }
ec30109b 0:0797b0e47e6d 82 }
ec30109b 0:0797b0e47e6d 83