ARES
Time is good
Fork of
Robot2016_2-0
by 
ARES
Revision 99:1fcb088f8f36, committed 2016-05-07
- Comitter:
- Jagang
- Date:
- Sat May 07 02:15:23 2016 +0000
- Parent:
- 98:2426d699362b
- Child:
- 100:7c7174048302
- Commit message:
- 75pts (? tester avec le bras r?par?)
Changed in this revision
--- a/ControlleurPince/ControlleurPince.cpp Fri May 06 23:55:01 2016 +0000
+++ b/ControlleurPince/ControlleurPince.cpp Sat May 07 02:15:23 2016 +0000
@@ -1,6 +1,16 @@
#include "ControlleurPince.h"
+float max(float a, float b)
+{
+ return a<b?b:a;
+}
+
+float min(float a, float b)
+{
+ return a<b?a:b;
+}
+
ControlleurPince::ControlleurPince(Stepper &p_RMot, Stepper &p_ZMot, Stepper &p_LMot, DigitalIn &p_EnR, DigitalIn &p_EnZ, DigitalIn &p_EnL, AX12 &_Lax12, AX12 &_Rax12) :
RMot(p_RMot), ZMot(p_ZMot), LMot(p_LMot), EnR(p_EnR), EnZ(p_EnZ), EnL(p_EnL), Lax12(_Lax12), Rax12(_Rax12)
{
@@ -44,8 +54,9 @@
void ControlleurPince::setPos(float z, float delta, float center)
{
- if(z > 0.f && z < 100.f)
+ if(z >= 0.f)
{
+ z = min(130,z);
ZMot.mm(z-pos_z,true);
pos_z = z;
}
@@ -53,7 +64,7 @@
if(delta >= 0 || center >= -500)
{
if(delta < 0)
- delta = 130 - pos_r - pos_l;
+ delta = 150 - pos_r - pos_l;
if(center < -500)
center = - pos_r/2 + pos_l/2;
@@ -82,15 +93,28 @@
void ControlleurPince::close()
{
- Lax12.setGoal(150);
+ /*Lax12.setGoal(150);
Rax12.setGoal(150);
- wait(2);
+ wait(2);*/
+
+ angle(0);
}
void ControlleurPince::open()
{
- Lax12.setGoal(46);
+ /*Lax12.setGoal(46);
Rax12.setGoal(248);
+ wait(2);*/
+
+ angle(100);
+}
+
+void ControlleurPince::angle(int a)
+{
+ a = min(a,100);
+
+ Lax12.setGoal(150-a);
+ Rax12.setGoal(150+a);
wait(2);
}
\ No newline at end of file
--- a/ControlleurPince/ControlleurPince.h Fri May 06 23:55:01 2016 +0000
+++ b/ControlleurPince/ControlleurPince.h Sat May 07 02:15:23 2016 +0000
@@ -17,10 +17,11 @@
void init();
void home(bool r=true, bool z=true, bool l=true);
- void setPos(float z, float delta, float center = -600);
+ void setPos(float z, float delta = -1, float center = -600);
void close();
void open();
+ void angle(int a);
private:
--- a/deplacement.cpp Fri May 06 23:55:01 2016 +0000
+++ b/deplacement.cpp Sat May 07 02:15:23 2016 +0000
@@ -68,7 +68,7 @@
roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
- wait(3);
+ wait(3.5);
wait_ms(10);
roboclaw.ResetEnc();
wait_ms(10);
--- a/main.cpp Fri May 06 23:55:01 2016 +0000
+++ b/main.cpp Sat May 07 02:15:23 2016 +0000
@@ -49,43 +49,111 @@
init_globals();
if (SCouleur == VERT) {
+ // Chateau
end.attach(&endFonc, 90);
- GotoDist(9.0);
- GotoArr(9.2);
+ GotoDist(9.0,2000,2000,2000);
+
+ // Callage
+ GotoArr(5);
R_SEUIL_SHARP = 0.25;
- GotoDist(2.5);
+
+ // Cabane 1
+ GotoDist(1.25);
GotoThet(-PI/2.f);
R_SEUIL_SHARP = 0.35;
- GotoDist(5.5);
+ GotoDist(2.75);
R_SEUIL_SHARP = 1;
- GotoDist(4.5);
- GotoArr(3);
+ controlleurPince.setPos(-1,150);
+ GotoDist(2.25);
+
+ // Cabane 2
+ GotoArr(2);
R_SEUIL_SHARP = 0.35;
GotoThet(PI/2.f);
+ GotoDist(1.75);
+ GotoThet(PI/2.f);
+ GotoArr(3);
+
+ // Coquillage 1
+ R_SEUIL_SHARP = 0.35;
GotoDist(3.5);
- GotoThet(-PI/2.f);
- R_SEUIL_SHARP = 1;
- GotoDist(4.6);
+ controlleurPince.setPos(0);
+ controlleurPince.angle(50);
+
+ GotoThet(-0.3f);
+
+ GotoDist(2);
+
+ controlleurPince.close();
+ controlleurPince.setPos(-1,70);
+
+ // Coquillage 2
+ GotoThet(PI/2.f-0.3f);
+ R_SEUIL_SHARP = 1;
+ controlleurPince.angle(50);
+ GotoDist(2);
+ controlleurPince.close();
+
+ // Fin !!!
+ GotoThet(PI/2.f);
+ GotoDist(1.75);
+ controlleurPince.angle(50);
+ GotoArr(1);
+
}
else// if (SCouleur == VIOLET)
{
+
+ // Chateau
end.attach(&endFonc, 90);
GotoDist(9.0,2000,2000,2000);
+
+ // Callage
GotoArr(5);
R_SEUIL_SHARP = 0.25;
+
+ // Cabane 1
GotoDist(1.25);
GotoThet(PI/2.f);
R_SEUIL_SHARP = 0.35;
GotoDist(2.75);
R_SEUIL_SHARP = 1;
+ controlleurPince.setPos(-1,150);
GotoDist(2.25);
+
+ // Cabane 2
GotoArr(2);
R_SEUIL_SHARP = 0.35;
GotoThet(-PI/2.f);
GotoDist(1.75);
- GotoThet(PI/2.f);
- R_SEUIL_SHARP = 1;
- GotoDist(2.3);
+ GotoThet(-PI/2.f);
+ GotoArr(3);
+
+ // Coquillage 1
+ R_SEUIL_SHARP = 0.35;
+ GotoDist(3.5);
+ controlleurPince.setPos(0);
+ controlleurPince.angle(50);
+
+ GotoThet(0.3f);
+
+ GotoDist(2);
+
+ controlleurPince.close();
+ controlleurPince.setPos(-1,70);
+
+ // Coquillage 2
+ GotoThet(-PI/2.f+0.3f);
+ R_SEUIL_SHARP = 1;
+ controlleurPince.angle(50);
+ GotoDist(2);
+ controlleurPince.close();
+
+ // Fin !!!
+ GotoThet(-PI/2.f);
+ GotoDist(1.75);
+ controlleurPince.angle(50);
+ GotoArr(1);
}
/* else {
GotoDistPos(200);
@@ -138,9 +206,9 @@
roboclaw.ForwardM1(0);
roboclaw.ForwardM2(0);
wait(1);
- Parasol.setMaxTorque(1000);
+ Parasol.setGoal(300);
wait(1);
- Parasol.setGoal(300);
+ Parasol.setMaxTorque(1000);
while(1);
}
@@ -148,16 +216,22 @@
roboclaw.ForwardM1(0);
roboclaw.ForwardM2(0);
+
+ controlleurPince.init();
Parasol.setMode(0);
Parasol.setMaxTorque(200);
Parasol.setGoal(150);
- wait(1);
+ wait(0.5);
Parasol.setGoal(160);
- wait(1);
+ wait(0.5);
Parasol.setGoal(150);
wait(1);
Parasol.setMaxTorque(0);
+ controlleurPince.home();
+ controlleurPince.setPos(130,0,0);
+ controlleurPince.open();
+
while(START == 1)
{
LEDR = 1;