ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

ControlleurPince/ControlleurPince.cpp@92:f09f55aa992b, 2016-05-06 (annotated)

Committer:
IceTeam
Date:
Fri May 06 15:25:21 2016 +0000
Revision:
92:f09f55aa992b
Child:
93:c0b040954eac
Rajout de ControlleurPince & StepperMotor;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 92:f09f55aa992b 1
IceTeam 92:f09f55aa992b 2 #include "defines.h"
IceTeam 92:f09f55aa992b 3 #include "ControlleurPince.h"
IceTeam 92:f09f55aa992b 4
IceTeam 92:f09f55aa992b 5 ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12) :
IceTeam 92:f09f55aa992b 6 RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL), Lax12(_Lax12), Rax12(_Rax12)
IceTeam 92:f09f55aa992b 7 {
IceTeam 92:f09f55aa992b 8 pos_r = 0;
IceTeam 92:f09f55aa992b 9 pos_z = 0;
IceTeam 92:f09f55aa992b 10 pos_l = 0;
IceTeam 92:f09f55aa992b 11
IceTeam 92:f09f55aa992b 12
IceTeam 92:f09f55aa992b 13 }
IceTeam 92:f09f55aa992b 14
IceTeam 92:f09f55aa992b 15 void ControlleurPince::init()
IceTeam 92:f09f55aa992b 16 {
IceTeam 92:f09f55aa992b 17 Lax12.setMode(0);
IceTeam 92:f09f55aa992b 18 wait(0.01);
IceTeam 92:f09f55aa992b 19 Rax12.setMode(0);
IceTeam 92:f09f55aa992b 20 wait(0.01);
IceTeam 92:f09f55aa992b 21
IceTeam 92:f09f55aa992b 22 Lax12.setMaxTorque(200);
IceTeam 92:f09f55aa992b 23 wait(0.01);
IceTeam 92:f09f55aa992b 24 Rax12.setMaxTorque(200);
IceTeam 92:f09f55aa992b 25 wait(0.01);
IceTeam 92:f09f55aa992b 26 }
IceTeam 92:f09f55aa992b 27
IceTeam 92:f09f55aa992b 28
IceTeam 92:f09f55aa992b 29 void ControlleurPince::home(bool r, bool z, bool l)
IceTeam 92:f09f55aa992b 30 {
IceTeam 92:f09f55aa992b 31 if(r)
IceTeam 92:f09f55aa992b 32 {
IceTeam 92:f09f55aa992b 33 while(EnR==true) RMot.step(1, BAS, DELAY);
IceTeam 92:f09f55aa992b 34 pos_r = 0;
IceTeam 92:f09f55aa992b 35 }
IceTeam 92:f09f55aa992b 36 if(z)
IceTeam 92:f09f55aa992b 37 {
IceTeam 92:f09f55aa992b 38 while(EnZ==true) ZMot.step(1, BAS, DELAY);
IceTeam 92:f09f55aa992b 39 pos_z = 0;
IceTeam 92:f09f55aa992b 40 }
IceTeam 92:f09f55aa992b 41 if(l)
IceTeam 92:f09f55aa992b 42 {
IceTeam 92:f09f55aa992b 43 while(EnL==true) LMot.step(1, BAS, DELAY);
IceTeam 92:f09f55aa992b 44 pos_l = 0;
IceTeam 92:f09f55aa992b 45 }
IceTeam 92:f09f55aa992b 46 }
IceTeam 92:f09f55aa992b 47
IceTeam 92:f09f55aa992b 48 void ControlleurPince::setPos(float z, float delta, float center)
IceTeam 92:f09f55aa992b 49 {
IceTeam 92:f09f55aa992b 50 if(z > 0.f && z < 100.f)
IceTeam 92:f09f55aa992b 51 {
IceTeam 92:f09f55aa992b 52 ZMot.mm(z-pos_z,true);
IceTeam 92:f09f55aa992b 53 pos_z = z;
IceTeam 92:f09f55aa992b 54 }
IceTeam 92:f09f55aa992b 55
IceTeam 92:f09f55aa992b 56 if(delta >= 0 || center >= -500)
IceTeam 92:f09f55aa992b 57 {
IceTeam 92:f09f55aa992b 58 if(delta < 0)
IceTeam 92:f09f55aa992b 59 delta = 130 - pos_r - pos_l;
IceTeam 92:f09f55aa992b 60 if(center < -500)
IceTeam 92:f09f55aa992b 61 center = - pos_r/2 + pos_l/2;
IceTeam 92:f09f55aa992b 62
IceTeam 92:f09f55aa992b 63
IceTeam 92:f09f55aa992b 64
IceTeam 92:f09f55aa992b 65 /*
IceTeam 92:f09f55aa992b 66 => pos_l = center*2 + pos_r
IceTeam 92:f09f55aa992b 67 delta = 130 - pos_r - center*2 - pos_r
IceTeam 92:f09f55aa992b 68 => 2*pos_r = 130 - center*2 - delta
IceTeam 92:f09f55aa992b 69 => pos_r = (130-delta)/2 - center
IceTeam 92:f09f55aa992b 70 => pos_l = (130-delta)/2 + center
IceTeam 92:f09f55aa992b 71 */
IceTeam 92:f09f55aa992b 72
IceTeam 92:f09f55aa992b 73 float r = (130.f-delta)/2.f - center;
IceTeam 92:f09f55aa992b 74 float l = (130.f-delta)/2.f + center;
IceTeam 92:f09f55aa992b 75
IceTeam 92:f09f55aa992b 76 RMot.mm(r-pos_r,true);
IceTeam 92:f09f55aa992b 77 pos_r = r;
IceTeam 92:f09f55aa992b 78
IceTeam 92:f09f55aa992b 79 LMot.mm(l-pos_l,true);
IceTeam 92:f09f55aa992b 80 pos_l = l;
IceTeam 92:f09f55aa992b 81
IceTeam 92:f09f55aa992b 82 }
IceTeam 92:f09f55aa992b 83
IceTeam 92:f09f55aa992b 84 while(! (RMot.done() && ZMot.done() && LMot.done()))
IceTeam 92:f09f55aa992b 85 wait(DELAY);
IceTeam 92:f09f55aa992b 86 }
IceTeam 92:f09f55aa992b 87
IceTeam 92:f09f55aa992b 88 void ControlleurPince::close()
IceTeam 92:f09f55aa992b 89 {
IceTeam 92:f09f55aa992b 90 Lax12.setGoal(150);
IceTeam 92:f09f55aa992b 91 Rax12.setGoal(150);
IceTeam 92:f09f55aa992b 92 wait(0.2);
IceTeam 92:f09f55aa992b 93 }
IceTeam 92:f09f55aa992b 94
IceTeam 92:f09f55aa992b 95
IceTeam 92:f09f55aa992b 96 void ControlleurPince::open()
IceTeam 92:f09f55aa992b 97 {
IceTeam 92:f09f55aa992b 98 Lax12.setGoal(100);
IceTeam 92:f09f55aa992b 99 Rax12.setGoal(200);
IceTeam 92:f09f55aa992b 100 wait(0.2);
IceTeam 92:f09f55aa992b 101 }