Romain Ame / Mbed 2 deprecated Stressed

coucou

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

ControlleurPince/ControlleurPince.cpp@76:cd8c7da76768, 2016-05-05 (annotated)

Committer:
IceTeam
Date:
Thu May 05 08:46:03 2016 +0000
Revision:
76:cd8c7da76768
Parent:
71:5590dbe8393a 
test

Who changed what in which revision?

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