Soma Takahashi / towelest

mecha

Dependents:   2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue

towelest.cpp@3:63bb3e19c7eb, 2019-09-17 (annotated)

Committer:
ec30109b
Date:
Tue Sep 17 02:10:43 2019 +0000
Revision:
3:63bb3e19c7eb
Parent:
2:623fb33351af
Child:
4:416d194973a3
New towelest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 0:0797b0e47e6d 1 #include "towelest.h"
ec30109b 0:0797b0e47e6d 2
ec30109b 3:63bb3e19c7eb 3 towelest::towelest(Serial* RS485,int address,PinName Limit0,PinName Limit1,PinName Limit2):
ec30109b 3:63bb3e19c7eb 4 pid(4.0, 0, 0.001, 0.01),
ec30109b 3:63bb3e19c7eb 5 LimitSW0(Limit0),
ec30109b 3:63bb3e19c7eb 6 LimitSW1(Limit1),
ec30109b 3:63bb3e19c7eb 7 LimitSW2(Limit2)
ec30109b 0:0797b0e47e6d 8 {
ec30109b 3:63bb3e19c7eb 9 Motor = new ikarashiMDC*[3];
ec30109b 3:63bb3e19c7eb 10 Motor[0] = new ikarashiMDC(address,0,SM,RS485);
ec30109b 3:63bb3e19c7eb 11 Motor[1] = new ikarashiMDC(address,1,SM,RS485);
ec30109b 3:63bb3e19c7eb 12 Motor[2] = new ikarashiMDC(address,2,SM,RS485);
ec30109b 3:63bb3e19c7eb 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 3:63bb3e19c7eb 17
ec30109b 0:0797b0e47e6d 18 pid.setInputLimits(-1800, 1800);
ec30109b 0:0797b0e47e6d 19 pid.setOutputLimits(-0.6,0.6);
ec30109b 0:0797b0e47e6d 20 pid.setBias(0.0);
ec30109b 0:0797b0e47e6d 21 pid.setMode(AUTO_MODE);
ec30109b 0:0797b0e47e6d 22 pid.setSetPoint(0);
ec30109b 0:0797b0e47e6d 23
ec30109b 0:0797b0e47e6d 24 }
ec30109b 0:0797b0e47e6d 25
ec30109b 0:0797b0e47e6d 26 void towelest::open()
ec30109b 0:0797b0e47e6d 27 {
ec30109b 3:63bb3e19c7eb 28 if(LimitSW0.read() == 1){
ec30109b 0:0797b0e47e6d 29 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 30 Motor[1]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 31 }else{
ec30109b 0:0797b0e47e6d 32 Motor[0]->setSpeed(-0.2);
ec30109b 0:0797b0e47e6d 33 Motor[1]->setSpeed(0);
ec30109b 0:0797b0e47e6d 34 }
ec30109b 0:0797b0e47e6d 35 }
ec30109b 2:623fb33351af 36 void towelest::close()
ec30109b 2:623fb33351af 37 {
ec30109b 2:623fb33351af 38 Motor[0]->setSpeed(0.2);
ec30109b 2:623fb33351af 39 Motor[1]->setSpeed(0);
ec30109b 2:623fb33351af 40 }
ec30109b 2:623fb33351af 41
ec30109b 0:0797b0e47e6d 42 void towelest::lift()
ec30109b 0:0797b0e47e6d 43 {
ec30109b 3:63bb3e19c7eb 44 if(LimitSW1.read() == 1){
ec30109b 0:0797b0e47e6d 45 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 46 Motor[1]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 47 }else{
ec30109b 0:0797b0e47e6d 48 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 49 Motor[1]->setSpeed(0.8);
ec30109b 0:0797b0e47e6d 50 }
ec30109b 0:0797b0e47e6d 51 }
ec30109b 0:0797b0e47e6d 52
ec30109b 3:63bb3e19c7eb 53 void towelest::descent()
ec30109b 3:63bb3e19c7eb 54 {
ec30109b 3:63bb3e19c7eb 55 if(LimitSW2.read() == 1){
ec30109b 3:63bb3e19c7eb 56 Motor[0]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 57 Motor[1]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 58 }else{
ec30109b 3:63bb3e19c7eb 59 Motor[0]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 60 Motor[1]->setSpeed(-0.8);
ec30109b 3:63bb3e19c7eb 61 }
ec30109b 3:63bb3e19c7eb 62 }
ec30109b 3:63bb3e19c7eb 63
ec30109b 0:0797b0e47e6d 64 void towelest::drop()
ec30109b 0:0797b0e47e6d 65 {
ec30109b 0:0797b0e47e6d 66 timer.start();
ec30109b 0:0797b0e47e6d 67 time = timer.read();
ec30109b 1:c6950f45b03c 68 if(time <= 5) {
ec30109b 0:0797b0e47e6d 69 pid.setSetPoint(-800);
ec30109b 0:0797b0e47e6d 70 } else {
ec30109b 0:0797b0e47e6d 71 pid.setSetPoint(0);
ec30109b 0:0797b0e47e6d 72 timer.stop();
ec30109b 0:0797b0e47e6d 73 timer.reset();
ec30109b 0:0797b0e47e6d 74 }
ec30109b 0:0797b0e47e6d 75 }
ec30109b 0:0797b0e47e6d 76
ec30109b 2:623fb33351af 77 void towelest::setPulse(int Loli)
ec30109b 2:623fb33351af 78 {
ec30109b 2:623fb33351af 79 loli = Loli;
ec30109b 2:623fb33351af 80 }
ec30109b 2:623fb33351af 81
ec30109b 3:63bb3e19c7eb 82 void towelest::pid_compute()
ec30109b 0:0797b0e47e6d 83 {
ec30109b 3:63bb3e19c7eb 84 pid.setProcessValue(loli);
ec30109b 3:63bb3e19c7eb 85 compute = pid.compute();
ec30109b 3:63bb3e19c7eb 86 Motor[2]->setSpeed(compute);
ec30109b 3:63bb3e19c7eb 87 //pc.printf("%d %d %f \n\r",Limit1,Limit2,Limit3,RotationDistance);
ec30109b 0:0797b0e47e6d 88 }
ec30109b 0:0797b0e47e6d 89
ec30109b 3:63bb3e19c7eb 90