Soma Takahashi / towelest

mecha

Dependents:   2019NHK_A_manual_red 2019NHK_A_manual_red 2019NHK_A_manual_blue

towelest.cpp@5:1d0e66198bb4, 2019-09-23 (annotated)

Committer:
ec30109b
Date:
Mon Sep 23 12:21:49 2019 +0000
Revision:
5:1d0e66198bb4
Parent:
4:416d194973a3
Child:
6:9f932b9adee5
new

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 5:1d0e66198bb4 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 3:63bb3e19c7eb 45 if(LimitSW1.read() == 1){
ec30109b 0:0797b0e47e6d 46 Motor[0]->setSpeed(0);
ec30109b 0:0797b0e47e6d 47 Motor[1]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 48 }else{
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 3:63bb3e19c7eb 54 void towelest::descent()
ec30109b 3:63bb3e19c7eb 55 {
ec30109b 3:63bb3e19c7eb 56 if(LimitSW2.read() == 1){
ec30109b 3:63bb3e19c7eb 57 Motor[0]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 58 Motor[1]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 59 }else{
ec30109b 3:63bb3e19c7eb 60 Motor[0]->setSpeed(0);
ec30109b 3:63bb3e19c7eb 61 Motor[1]->setSpeed(-0.8);
ec30109b 3:63bb3e19c7eb 62 }
ec30109b 3:63bb3e19c7eb 63 }
ec30109b 3:63bb3e19c7eb 64
ec30109b 4:416d194973a3 65 void towelest::setPoint(int target)
ec30109b 0:0797b0e47e6d 66 {
ec30109b 4:416d194973a3 67 pid.setSetPoint(target);
ec30109b 0:0797b0e47e6d 68 }
ec30109b 0:0797b0e47e6d 69
ec30109b 2:623fb33351af 70 void towelest::setPulse(int Loli)
ec30109b 2:623fb33351af 71 {
ec30109b 2:623fb33351af 72 loli = Loli;
ec30109b 2:623fb33351af 73 }
ec30109b 2:623fb33351af 74
ec30109b 3:63bb3e19c7eb 75 void towelest::pid_compute()
ec30109b 0:0797b0e47e6d 76 {
ec30109b 3:63bb3e19c7eb 77 pid.setProcessValue(loli);
ec30109b 3:63bb3e19c7eb 78 compute = pid.compute();
ec30109b 3:63bb3e19c7eb 79 Motor[2]->setSpeed(compute);
ec30109b 3:63bb3e19c7eb 80 //pc.printf("%d %d %f \n\r",Limit1,Limit2,Limit3,RotationDistance);
ec30109b 0:0797b0e47e6d 81 }
ec30109b 0:0797b0e47e6d 82
ec30109b 4:416d194973a3 83 void towelest::allstop()
ec30109b 4:416d194973a3 84 {
ec30109b 4:416d194973a3 85 Motor[0]->setSpeed(0);
ec30109b 4:416d194973a3 86 Motor[1]->setSpeed(0);
ec30109b 4:416d194973a3 87 Motor[2]->setSpeed(0);
ec30109b 4:416d194973a3 88 }
ec30109b 3:63bb3e19c7eb 89