ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

ControlleurPince/ControlleurPince.cpp@99:1fcb088f8f36, 2016-05-07 (annotated)

Committer:
Jagang
Date:
Sat May 07 02:15:23 2016 +0000
Revision:
99:1fcb088f8f36
Parent:
94:86b9bd6d5c28 
75pts (? tester avec le bras r?par?)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 92:f09f55aa992b 1
IceTeam 92:f09f55aa992b 2 #include "ControlleurPince.h"
IceTeam 92:f09f55aa992b 3
Jagang 99:1fcb088f8f36 4 float max(float a, float b)
Jagang 99:1fcb088f8f36 5 {
Jagang 99:1fcb088f8f36 6 return a<b?b:a;
Jagang 99:1fcb088f8f36 7 }
Jagang 99:1fcb088f8f36 8
Jagang 99:1fcb088f8f36 9 float min(float a, float b)
Jagang 99:1fcb088f8f36 10 {
Jagang 99:1fcb088f8f36 11 return a<b?a:b;
Jagang 99:1fcb088f8f36 12 }
Jagang 99:1fcb088f8f36 13
IceTeam 92:f09f55aa992b 14 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 15 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 16 {
IceTeam 92:f09f55aa992b 17 pos_r = 0;
IceTeam 92:f09f55aa992b 18 pos_z = 0;
IceTeam 92:f09f55aa992b 19 pos_l = 0;
IceTeam 92:f09f55aa992b 20 }
IceTeam 92:f09f55aa992b 21
IceTeam 92:f09f55aa992b 22 void ControlleurPince::init()
IceTeam 92:f09f55aa992b 23 {
IceTeam 92:f09f55aa992b 24 Lax12.setMode(0);
IceTeam 92:f09f55aa992b 25 wait(0.01);
IceTeam 92:f09f55aa992b 26 Rax12.setMode(0);
IceTeam 92:f09f55aa992b 27 wait(0.01);
IceTeam 92:f09f55aa992b 28
IceTeam 92:f09f55aa992b 29 Lax12.setMaxTorque(200);
IceTeam 92:f09f55aa992b 30 wait(0.01);
IceTeam 92:f09f55aa992b 31 Rax12.setMaxTorque(200);
IceTeam 92:f09f55aa992b 32 wait(0.01);
IceTeam 92:f09f55aa992b 33 }
IceTeam 92:f09f55aa992b 34
IceTeam 92:f09f55aa992b 35
IceTeam 92:f09f55aa992b 36 void ControlleurPince::home(bool r, bool z, bool l)
IceTeam 92:f09f55aa992b 37 {
IceTeam 92:f09f55aa992b 38 if(r)
IceTeam 92:f09f55aa992b 39 {
IceTeam 92:f09f55aa992b 40 while(EnR==true) RMot.step(1, BAS, DELAY);
IceTeam 92:f09f55aa992b 41 pos_r = 0;
IceTeam 92:f09f55aa992b 42 }
IceTeam 92:f09f55aa992b 43 if(z)
IceTeam 92:f09f55aa992b 44 {
IceTeam 92:f09f55aa992b 45 while(EnZ==true) ZMot.step(1, BAS, DELAY);
IceTeam 92:f09f55aa992b 46 pos_z = 0;
IceTeam 92:f09f55aa992b 47 }
IceTeam 92:f09f55aa992b 48 if(l)
IceTeam 92:f09f55aa992b 49 {
IceTeam 92:f09f55aa992b 50 while(EnL==true) LMot.step(1, BAS, DELAY);
IceTeam 92:f09f55aa992b 51 pos_l = 0;
IceTeam 92:f09f55aa992b 52 }
IceTeam 92:f09f55aa992b 53 }
IceTeam 92:f09f55aa992b 54
IceTeam 92:f09f55aa992b 55 void ControlleurPince::setPos(float z, float delta, float center)
IceTeam 92:f09f55aa992b 56 {
Jagang 99:1fcb088f8f36 57 if(z >= 0.f)
IceTeam 92:f09f55aa992b 58 {
Jagang 99:1fcb088f8f36 59 z = min(130,z);
IceTeam 92:f09f55aa992b 60 ZMot.mm(z-pos_z,true);
IceTeam 92:f09f55aa992b 61 pos_z = z;
IceTeam 92:f09f55aa992b 62 }
IceTeam 92:f09f55aa992b 63
IceTeam 92:f09f55aa992b 64 if(delta >= 0 || center >= -500)
IceTeam 92:f09f55aa992b 65 {
IceTeam 92:f09f55aa992b 66 if(delta < 0)
Jagang 99:1fcb088f8f36 67 delta = 150 - pos_r - pos_l;
IceTeam 92:f09f55aa992b 68 if(center < -500)
IceTeam 93:c0b040954eac 69 center = - pos_r/2 + pos_l/2;
IceTeam 92:f09f55aa992b 70
IceTeam 92:f09f55aa992b 71 /*
IceTeam 92:f09f55aa992b 72 => pos_l = center*2 + pos_r
IceTeam 92:f09f55aa992b 73 delta = 130 - pos_r - center*2 - pos_r
IceTeam 92:f09f55aa992b 74 => 2*pos_r = 130 - center*2 - delta
IceTeam 92:f09f55aa992b 75 => pos_r = (130-delta)/2 - center
IceTeam 92:f09f55aa992b 76 => pos_l = (130-delta)/2 + center
IceTeam 92:f09f55aa992b 77 */
IceTeam 92:f09f55aa992b 78
IceTeam 94:86b9bd6d5c28 79 float r = (150.f-delta)/2.f - center;
IceTeam 94:86b9bd6d5c28 80 float l = (150.f-delta)/2.f + center;
IceTeam 92:f09f55aa992b 81
IceTeam 92:f09f55aa992b 82 RMot.mm(r-pos_r,true);
IceTeam 92:f09f55aa992b 83 pos_r = r;
IceTeam 92:f09f55aa992b 84
IceTeam 92:f09f55aa992b 85 LMot.mm(l-pos_l,true);
IceTeam 92:f09f55aa992b 86 pos_l = l;
IceTeam 92:f09f55aa992b 87
IceTeam 92:f09f55aa992b 88 }
IceTeam 92:f09f55aa992b 89
IceTeam 92:f09f55aa992b 90 while(! (RMot.done() && ZMot.done() && LMot.done()))
IceTeam 92:f09f55aa992b 91 wait(DELAY);
IceTeam 92:f09f55aa992b 92 }
IceTeam 92:f09f55aa992b 93
IceTeam 92:f09f55aa992b 94 void ControlleurPince::close()
IceTeam 92:f09f55aa992b 95 {
Jagang 99:1fcb088f8f36 96 /*Lax12.setGoal(150);
IceTeam 92:f09f55aa992b 97 Rax12.setGoal(150);
Jagang 99:1fcb088f8f36 98 wait(2);*/
Jagang 99:1fcb088f8f36 99
Jagang 99:1fcb088f8f36 100 angle(0);
IceTeam 92:f09f55aa992b 101 }
IceTeam 92:f09f55aa992b 102
IceTeam 92:f09f55aa992b 103
IceTeam 92:f09f55aa992b 104 void ControlleurPince::open()
IceTeam 92:f09f55aa992b 105 {
Jagang 99:1fcb088f8f36 106 /*Lax12.setGoal(46);
IceTeam 94:86b9bd6d5c28 107 Rax12.setGoal(248);
Jagang 99:1fcb088f8f36 108 wait(2);*/
Jagang 99:1fcb088f8f36 109
Jagang 99:1fcb088f8f36 110 angle(100);
Jagang 99:1fcb088f8f36 111 }
Jagang 99:1fcb088f8f36 112
Jagang 99:1fcb088f8f36 113 void ControlleurPince::angle(int a)
Jagang 99:1fcb088f8f36 114 {
Jagang 99:1fcb088f8f36 115 a = min(a,100);
Jagang 99:1fcb088f8f36 116
Jagang 99:1fcb088f8f36 117 Lax12.setGoal(150-a);
Jagang 99:1fcb088f8f36 118 Rax12.setGoal(150+a);
IceTeam 94:86b9bd6d5c28 119 wait(2);
IceTeam 92:f09f55aa992b 120 }