Guillaume F4HDK
scribing robot see https://hackaday.io/project/153194/
See description of the project here: https://hackaday.io/project/153194/
Diff: main.cpp
- Revision:
- 1:163fa4a925d9
- Parent:
- 0:694620b4c620
--- a/main.cpp Sun Apr 22 13:57:53 2018 +0000
+++ b/main.cpp Sun Apr 22 14:00:17 2018 +0000
@@ -1,55 +1,55 @@
#include "mbed.h"
#include "time.h"
-#define servo_stylo_pos 1800//1800
-#define servo_stylo_lev 1680//1680
-#define servo_neutre 1400
-#define servo_gomme_lev 900//900
-#define servo_gomme_pos 755//730 780
+#define laid_pen_servopos 1800//1800
+#define raised_pen_servopos 1680//1680
+#define neutral_pen_servopos 1400
+#define raised_eraser_servopos 900//900
+#define laid_eraser_servopos 755//730
DigitalOut myled(LED1);
DigitalOut led_err(LED3);
-DigitalOut stepper_clk_1(p21);//21
-DigitalOut stepper_dir_1(p22);//22
-DigitalOut stepper_clk_2(p23);//23
-DigitalOut stepper_dir_2(p24);//24
+DigitalOut stepper_clk_1(p21);
+DigitalOut stepper_dir_1(p22);
+DigitalOut stepper_clk_2(p23);
+DigitalOut stepper_dir_2(p24);
DigitalOut servoout(p17);
AnalogIn potard_1 (p20);
-DigitalIn switch_epaule(p18);
-DigitalIn switch_coude(p19);
+DigitalIn switch_shoulder(p18);
+DigitalIn switch_elbow(p19);
Serial pc(USBTX, USBRX);
LocalFileSystem local("local");
Timeout pulse_stepper1;
Timeout pulse_stepper2;
Timeout servo_timeout;
-Ticker appel_servo;
+Ticker servo_call;
-int cons_servo_main=1400; //1800
-int a_coude, a_epaule, dir_1_real, dir_2_real, tick_attached_coude, tick_attached_epaule;
-int period_stepper1, period_stepper2, precis, outil, ligne;
+int wrist_servo_setpoint=1400; //1800
+int a_elbow, a_shoulder, dir_1_real, dir_2_real, tick_attached_elbow, tick_attached_shoulder;
+int period_stepper1, period_stepper2, accurate, tool, line;
float X_pos, Y_pos;
-float X_print, Y_print, X_fictif;
-int remplissage[5]= {0,0,0,0,0};
-char texte_heure_bk[10];
-char texte_minute_bk[10];
+float X_print, Y_print, X_fictive;
+int line_filling_st[5]= {0,0,0,0,0};
+char hour_txt_back[10];
+char minute_txt_back[10];
-float taille_glob;
+float glob_size;
-char police_maj[256][30]= {
- 5,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //32 espace
+char font[256][30]= {
+ 5,8,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //32 space
,2,2,15,3,2,5,6,2,1,5,4,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //33 !
,2,1,15,3,1,11,2,4,15,3,4,11,5,5,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //34 "
,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //35
,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //36
,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //37 %
,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //38 &
- ,2,3,15,3,1,11,5,4,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //39 ' appostrophe
+ ,2,3,15,3,1,11,5,4,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //39 ' apostrophe
,4,11,8,20,14,18,5,5,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //40 (
,4,-6,8,20,10,6,5,5,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //41 )
,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //42
,2,1,5,3,9,5,2,5,9,3,5,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //43 +
- ,2,3,1,3,1,-3,5,4,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //44 , virgule
+ ,2,3,1,3,1,-3,5,4,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //44 , comma
,2,2,5,3,8,5,5,10,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //45 -
,6,2,1,5,4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //46 . point
,2,9,15,3,1,1,5,10,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //47 /
@@ -271,14 +271,14 @@
void tick_servo (void)
{
servoout=1;
- servo_timeout.attach_us(&stop_tick_servo, cons_servo_main);
+ servo_timeout.attach_us(&stop_tick_servo, wrist_servo_setpoint);
}
void clk_stepper1(void)
{
stepper_clk_1=1;
pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
- a_coude=a_coude + dir_1_real;
+ a_elbow=a_elbow + dir_1_real;
stepper_clk_1=0;
}
@@ -286,50 +286,50 @@
{
stepper_clk_2=1;
pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
- a_epaule=a_epaule + dir_2_real;
+ a_shoulder=a_shoulder + dir_2_real;
stepper_clk_2=0;
}
void tick_detach()
{
- tick_attached_coude=0;
- tick_attached_epaule=0;
+ tick_attached_elbow=0;
+ tick_attached_shoulder=0;
pulse_stepper1.detach();
pulse_stepper2.detach();
}
-void set_poignet(int pospoignet)
+void set_wrist(int wrist_pos)
{
- switch (pospoignet) {
- case 1 : //stylo pose
- if (cons_servo_main!=servo_stylo_pos) {
+ switch (wrist_pos) {
+ case 1 : //laid pen
+ if (wrist_servo_setpoint!=laid_pen_servopos) {
wait_ms(100);
- cons_servo_main=servo_stylo_pos;
+ wrist_servo_setpoint=laid_pen_servopos;
myled=1;
wait_ms(80);
}
- precis=1;
+ accurate=1;
break;
- case 2 : //stylo leve
- if (cons_servo_main!=servo_stylo_lev) {
+ case 2 : //raised pen
+ if (wrist_servo_setpoint!=raised_pen_servopos) {
//wait_ms(50);
- cons_servo_main=servo_stylo_lev;
+ wrist_servo_setpoint=raised_pen_servopos;
myled=0;
wait_ms(50);
}
- precis=0;
+ accurate=0;
break;
- case 3 : //neutre
- cons_servo_main=servo_neutre;
- precis=0;
+ case 3 : //neutral
+ wrist_servo_setpoint=neutral_pen_servopos;
+ accurate=0;
break;
- case 4 : //gomme levee
- cons_servo_main=servo_gomme_lev;
- precis=0;
+ case 4 : //eraser raised
+ wrist_servo_setpoint=raised_eraser_servopos;
+ accurate=0;
break;
- case 5 : //gomme posee
- cons_servo_main=servo_gomme_pos;
- precis=2;
+ case 5 : //eraser laid
+ wrist_servo_setpoint=laid_eraser_servopos;
+ accurate=2;
}
}
@@ -339,7 +339,7 @@
{
//int arret=0;
int i;
- while (switch_epaule==1) {
+ while (switch_shoulder==1) {
stepper_clk_2=1;
wait_us(10);
stepper_clk_2=0;
@@ -352,14 +352,14 @@
stepper_clk_2=0;
wait_ms(1);
}
- while (switch_coude==1) {
+ while (switch_elbow==1) {
stepper_clk_1=1;
wait_us(10);
stepper_clk_1=0;
wait_ms(3);
}
- a_coude=-2940;//2900
- a_epaule=0;
+ a_elbow=-2940;//2900
+ a_shoulder=0;
//stepper_dir_1=1;
// for (i=1;i<2900;i++) {
// stepper_clk_1=1;
@@ -367,163 +367,146 @@
// stepper_clk_1=0;
// wait_ms(1);
//}
- //cons_servo_main=1800;
+ //wrist_servo_setpoint=1800;
}
-void setpos_ang (int a_coude_cible, int a_epaule_cible, int delay_ms)
+void setpos_ang (int a_elbow_target, int a_shoulder_target, int delay_ms)
{
- int err_a_coude, err_a_epaule;
+ int err_a_elbow, err_a_shoulder;
float delay_1, delay_2;
//pulse_stepper1.detach();
//pulse_stepper2.detach();
- err_a_coude=a_coude_cible - a_coude;
- err_a_epaule=a_epaule_cible - a_epaule;
- if (err_a_coude>0) {
+ err_a_elbow=a_elbow_target - a_elbow;
+ err_a_shoulder=a_shoulder_target - a_shoulder;
+ if (err_a_elbow>0) {
stepper_dir_1=1;
dir_1_real=1;
} else {
stepper_dir_1=0;
dir_1_real=-1;
};
- if (err_a_epaule>0) {
+ if (err_a_shoulder>0) {
stepper_dir_2=1;
dir_2_real=1;
} else {
stepper_dir_2=0;
dir_2_real=-1;
};
- err_a_coude=abs(err_a_coude);
- err_a_epaule=abs(err_a_epaule);
- //pc.printf("err
- if (err_a_coude !=0) { //laisser imperativement !=0
- delay_1=delay_ms*1000/err_a_coude;
- //printf("delay1 : %f\r\n",delay_1);
+ err_a_elbow=abs(err_a_elbow);
+ err_a_shoulder=abs(err_a_shoulder);
+ if (err_a_elbow !=0) {
+ delay_1=delay_ms*1000/err_a_elbow;
if (delay_1<250) {
pc.printf("X:%f Y:%f\r\n",X_pos, Y_pos);
printf("delay1 : %f\r\n",delay_1);
- printf("a_coude:%d a_coude_cible: %d err_coude : %d\r\n\r\n",a_coude, a_coude_cible, err_a_coude);
+ printf("a_elbow:%d a_elbow_target: %d err_elbow : %d\r\n\r\n",a_elbow, a_elbow_target, err_a_elbow);
delay_1=250;
led_err=1;
- //wait(1);
led_err=0;
}
period_stepper1=delay_1-3;
- if (tick_attached_coude==0) {
+ if (tick_attached_elbow==0) {
pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
- tick_attached_coude=1;
+ tick_attached_elbow=1;
}
} else {
- tick_attached_coude=0;
+ tick_attached_elbow=0;
pulse_stepper1.detach();
}
- if (err_a_epaule!=0) { //laisser imperativement !=0
- delay_2=delay_ms*1000/err_a_epaule;
- //printf("delay2 : %f\r\n",delay_2);
+ if (err_a_shoulder!=0) {
+ delay_2=delay_ms*1000/err_a_shoulder;
if (delay_2<250) {
pc.printf("X:%f Y:%f\r\n",X_pos, Y_pos);
printf("delay2 : %f\r\n",delay_2);
- printf("a_epaule:%d a_epaule_cible:%d err_epaul : %d\r\n\r\n",a_epaule, a_epaule_cible, err_a_epaule);
+ printf("a_shoulder:%d a_shoulder_target:%d err_epaul : %d\r\n\r\n",a_shoulder, a_shoulder_target, err_a_shoulder);
delay_2=250;
led_err=1;
- //wait(1);
led_err=0;
}
period_stepper2=delay_2-3;
- if (tick_attached_epaule==0) {
+ if (tick_attached_shoulder==0) {
pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
- tick_attached_epaule=1;
+ tick_attached_shoulder=1;
}
} else {
- tick_attached_epaule=0;
+ tick_attached_shoulder=0;
pulse_stepper2.detach();
}
- //tick_attached=1;
wait_ms(delay_ms);
- //pulse_stepper1.detach();
- //pulse_stepper2.detach();
- //pc.printf("Reelles coude: %d ; epaule: %d\r\n\r\n", a_coude, a_epaule);
}
-void goto_2D_short (float X_cible, float Y_cible, int delay_ms)
+void goto_2D_short (float X_target, float Y_target, int delay_ms)
{
- float allonge, deltax, deltay, alpha_D, arad_coude_cible, arad_epaule_cible, consigne_coude, consigne_epaule, erreur_coude, erreur_epaule;
+ float arm_extension, deltax, deltay, alpha_D, arad_elbow_target, arad_shoulder_target, setpoint_elbow, setpoint_shoulder, error_elbow, error_shoulder;
int delay_loc;
- // inversion matrice simplifiee
- if (X_cible > 360) {
- X_cible=360;
+ // simplified inverse kinematics
+ if (X_target > 360) {
+ X_target=360;
}
- if (X_cible < 0) {
- X_cible=0;
+ if (X_target < 0) {
+ X_target=0;
}
- if (Y_cible > 145) {
- Y_cible=145; //110
+ if (Y_target > 145) {
+ Y_target=145; //110
}
- if (Y_cible < 0) {
- Y_cible=0;
+ if (Y_target < 0) {
+ Y_target=0;
}
- deltax=X_cible-180;
- deltay=235-Y_cible;//220
- allonge=sqrt((deltax*deltax)+(deltay*deltay));
- arad_coude_cible=2*acos(allonge/320);// inverser le signe quand passage a consigne
+ deltax=X_target-180;
+ deltay=235-Y_target;//220
+ arm_extension=sqrt((deltax*deltax)+(deltay*deltay));
+ arad_elbow_target=2*acos(arm_extension/320);
alpha_D=atan(deltax/deltay);
- //conversion en consigne brutes (pas)
- if (outil==0) { //stylo
- arad_epaule_cible=alpha_D+(arad_coude_cible/2);
- consigne_epaule = arad_epaule_cible*1120.5;
- consigne_coude = consigne_epaule/4.4 -arad_coude_cible*1120.5;
+ //conversion to steps
+ if (tool==0) { //pen
+ arad_shoulder_target=alpha_D+(arad_elbow_target/2);
+ setpoint_shoulder = arad_shoulder_target*1120.5;
+ setpoint_elbow = setpoint_shoulder/4.4 -arad_elbow_target*1120.5;
} else {
- arad_epaule_cible=alpha_D-(arad_coude_cible/2); //gomme
- consigne_epaule = arad_epaule_cible*1120.5;
- consigne_coude = consigne_epaule/4.4 +arad_coude_cible*1120.5-850;
+ arad_shoulder_target=alpha_D-(arad_elbow_target/2); //gomme
+ setpoint_shoulder = arad_shoulder_target*1120.5;
+ setpoint_elbow = setpoint_shoulder/4.4 +arad_elbow_target*1120.5-850;
}
- //pc.printf("consigne_coude:%d ; epaule:%d\r\n", (int)consigne_coude, (int)consigne_epaule);
- //pc.printf("X:%f Y:%f\r\n", X_cible, Y_cible);
-
if (delay_ms==0) {
- erreur_coude=abs(consigne_coude-a_coude);
- erreur_epaule=abs(consigne_epaule-a_epaule);
- //delay_loc=0.5*max(erreur_coude, erreur_epaule);
- if (erreur_coude>erreur_epaule) {
- delay_loc=0.75*erreur_coude;
+ error_elbow=abs(setpoint_elbow-a_elbow);
+ error_shoulder=abs(setpoint_shoulder-a_shoulder);
+ if (error_elbow>error_shoulder) {
+ delay_loc=0.75*error_elbow;
} else {
- delay_loc=0.75*erreur_epaule;
+ delay_loc=0.75*error_shoulder;
}
- setpos_ang (consigne_coude, consigne_epaule, delay_loc);
- X_pos=X_cible;
- Y_pos=Y_cible;
+ setpos_ang (setpoint_elbow, setpoint_shoulder, delay_loc);
+ X_pos=X_target;
+ Y_pos=Y_target;
} else {
- setpos_ang (consigne_coude, consigne_epaule, delay_ms);
- X_pos=X_cible;
- Y_pos=Y_cible;
- //cinematique directe:
+ setpos_ang (setpoint_elbow, setpoint_shoulder, delay_ms);
+ X_pos=X_target;
+ Y_pos=Y_target;
}
}
-void goto_2D_line (float X_cible, float Y_cible)
+void goto_2D_line (float X_target, float Y_target)
{
float delta_x, delta_y, distance, x_decomp, y_decomp;
- int nb_pas, i;
- delta_x=X_cible-X_pos;
- delta_y=Y_cible-Y_pos;
+ int step_nb, i;
+ delta_x=X_target-X_pos;
+ delta_y=Y_target-Y_pos;
distance=sqrt((delta_x*delta_x)+(delta_y*delta_y));
- nb_pas = ceil (distance*1);// 1 pas = 1mm donc pas de facteur
- //pc.printf ("nb pas: %i\r\n", nb_pas);
- delta_x=delta_x/nb_pas;
- delta_y=delta_y/nb_pas;
+ step_nb = ceil (distance*1);// 1 step = 1 mm
+ delta_x=delta_x/step_nb;
+ delta_y=delta_y/step_nb;
x_decomp=X_pos;
y_decomp=Y_pos;
- //tick_detach();
- for (i=1; i<=nb_pas; i++) {
+ for (i=1; i<=step_nb; i++) {
x_decomp=x_decomp+delta_x;
y_decomp=y_decomp+delta_y;
- //pc.printf ("decomp: X: %f Y: %f\r\n", x_decomp, y_decomp);
- if(precis==1) {
+ if(accurate==1) {
goto_2D_short (x_decomp, y_decomp, 10+70*potard_1.read()); //12
- } else if (precis==2) {
+ } else if (accurate==2) {
goto_2D_short (x_decomp, y_decomp,13);
} else {
goto_2D_short (x_decomp, y_decomp, 9);
@@ -532,175 +515,164 @@
tick_detach();
}
-void circle_2D(float X_centre, float Y_centre, float rayon, float angle_deb, float angle_fin)
+void circle_2D(float X_centre, float Y_centre, float radius, float angle_deb, float angle_fin)
{
- float pas_angle, angle_actuel, x_decomp, y_decomp, delta_x, delta_y, distance;
- int nb_pas, i;
- nb_pas=1*ceil (rayon * abs (angle_fin-angle_deb));
- pas_angle=(angle_fin-angle_deb)/nb_pas;
- angle_actuel=angle_deb;
- x_decomp=X_centre + rayon*cos (angle_actuel);
- y_decomp=Y_centre + rayon*sin (angle_actuel);
+ float step_angle, current_angle, x_decomp, y_decomp, delta_x, delta_y, distance;
+ int step_nb, i;
+ step_nb=1*ceil (radius * abs (angle_fin-angle_deb));
+ step_angle=(angle_fin-angle_deb)/step_nb;
+ current_angle=angle_deb;
+ x_decomp=X_centre + radius*cos (current_angle);
+ y_decomp=Y_centre + radius*sin (current_angle);
delta_x=x_decomp-X_pos;
delta_y=y_decomp-Y_pos;
distance=sqrt((delta_x*delta_x)+(delta_y*delta_y));
- //pc.printf("decomp circle : X:%f Y:%f\r\n", x_decomp, y_decomp);
- //tick_detach();
if (distance>2) {
- //cons_servo_main=servo_stylo_lev;
- set_poignet(2);
- //wait_ms(100);
+ set_wrist(2);
goto_2D_line (x_decomp, y_decomp);
}
- set_poignet(1);
- //if (cons_servo_main!=servo_stylo_pos) {
- // //cons_servo_main=servo_stylo_pos;
- // set_poignet(1);
- // //wait_ms(100);
- // }
- //tick_detach();
- for (i=1; i<=nb_pas; i++) {
- angle_actuel=angle_actuel+pas_angle;
- x_decomp=X_centre + rayon*cos (angle_actuel);
- y_decomp=Y_centre + rayon*sin (angle_actuel);
- //pc.printf("decomp circle : X:%f Y:%f\r\n", x_decomp, y_decomp);
+ set_wrist(1);
+ for (i=1; i<=step_nb; i++) {
+ current_angle=current_angle+step_angle;
+ x_decomp=X_centre + radius*cos (current_angle);
+ y_decomp=Y_centre + radius*sin (current_angle);
goto_2D_short (x_decomp, y_decomp, 10+70*potard_1.read());
}
tick_detach();
}
-void set_outil(int outil_loc, float X_loc, float Y_loc)
+void set_tool(int tool_loc, float X_loc, float Y_loc)
{
- if ((outil_loc==1)&&(outil==0)) {//gomme
- set_poignet(3);
- outil=1;
+ if ((tool_loc==1)&&(tool==0)) {//eraser
+ set_wrist(3);
+ tool=1;
goto_2D_short (X_loc, Y_loc,0);//2000
tick_detach();
- set_poignet(4);
+ set_wrist(4);
wait_ms(200);
- } else if ((outil_loc==0)&&(outil==1)) {
- set_poignet(3);
- outil=0;
+ } else if ((tool_loc==0)&&(tool==1)) {
+ set_wrist(3);
+ tool=0;
goto_2D_short (X_loc, Y_loc,0);//2000
tick_detach();
- set_poignet(2);
+ set_wrist(2);
wait_ms(200);
- } else if ((outil_loc==1)&&(outil==1)) {
- set_poignet(4);
+ } else if ((tool_loc==1)&&(tool==1)) {
+ set_wrist(4);
}
}
-void print_lettre(char lettre, float taille)
+void scr_print_single_char(char charact, float size)
{
- int fin_lettre=0;
+ int s_end_char=0;
int i_instr=0;
- int x_temp, y_temp, lettre_loc;
+ int x_temp, y_temp, charact_loc;
float X_local, Y_local;
- float rayon_loc, angle_deb_loc, angle_fin_loc;
- lettre_loc=lettre;
- //printf ("char: %d", lettre+32);
- if (lettre_loc<0) {
- lettre_loc=lettre+256;
+ float radius_loc, angle_deb_loc, angle_fin_loc;
+ charact_loc=charact;
+ //printf ("char: %d", charact+32);
+ if (charact_loc<0) {
+ charact_loc=charact+256;
}
- //printf ("char: %d\r\n", lettre_loc+32);
- set_outil(0,X_print,Y_print);
- while (fin_lettre==0) {
- switch (police_maj[lettre_loc][i_instr]) {
- case 2 : // aller stylo leve
- //cons_servo_main=servo_stylo_lev;
- set_poignet(2);
+ //printf ("char: %d\r\n", charact_loc+32);
+ set_tool(0,X_print,Y_print);
+ while (s_end_char==0) {
+ switch (font[charact_loc][i_instr]) {
+ case 2 : // goto pen raised
+ //wrist_servo_setpoint=raised_pen_servopos;
+ set_wrist(2);
//wait_ms(100);
- X_local=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
- y_temp=police_maj[lettre_loc][i_instr+2];
+ X_local=X_print+size*(font[charact_loc][i_instr+1]);
+ y_temp=font[charact_loc][i_instr+2];
if (y_temp>127) {
y_temp=y_temp-256;
}
- Y_local=Y_print+taille*y_temp;
+ Y_local=Y_print+size*y_temp;
goto_2D_line(X_local, Y_local);
i_instr=i_instr+3;
break;
- case 3 : // ligne stylo pose
- set_poignet(1);
- //if (cons_servo_main!=servo_stylo_pos) {
- // cons_servo_main=servo_stylo_pos;
+ case 3 : // line pen laid
+ set_wrist(1);
+ //if (wrist_servo_setpoint!=laid_pen_servopos) {
+ // wrist_servo_setpoint=laid_pen_servopos;
// wait_ms(100);
//}
- X_local=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
- y_temp=police_maj[lettre_loc][i_instr+2];
+ X_local=X_print+size*(font[charact_loc][i_instr+1]);
+ y_temp=font[charact_loc][i_instr+2];
if (y_temp>127) {
y_temp=y_temp-256;
}
- Y_local=Y_print+taille*y_temp;
+ Y_local=Y_print+size*y_temp;
goto_2D_line(X_local, Y_local);
i_instr=i_instr+3;
break;
- case 4 : // cercle
- //X_local=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
- x_temp=police_maj[lettre_loc][i_instr+1];
+ case 4 : // circle
+ //X_local=X_print+size*(font[charact_loc][i_instr+1]);
+ x_temp=font[charact_loc][i_instr+1];
if (x_temp>127) {
x_temp=x_temp-256;
}
- X_local=X_print+taille*x_temp;
- y_temp=police_maj[lettre_loc][i_instr+2];
+ X_local=X_print+size*x_temp;
+ y_temp=font[charact_loc][i_instr+2];
if (y_temp>127) {
y_temp=y_temp-256;
}
- Y_local=Y_print+taille*y_temp;
- rayon_loc=taille*(police_maj[lettre_loc][i_instr+3])/2;
- angle_deb_loc=(0.3927*police_maj[lettre_loc][i_instr+4])-3.1416;
- angle_fin_loc=(0.3927*police_maj[lettre_loc][i_instr+5])-3.1416;
+ Y_local=Y_print+size*y_temp;
+ radius_loc=size*(font[charact_loc][i_instr+3])/2;
+ angle_deb_loc=(0.3927*font[charact_loc][i_instr+4])-3.1416;
+ angle_fin_loc=(0.3927*font[charact_loc][i_instr+5])-3.1416;
//pc.printf("angles: %f %f \r\n", angle_deb_loc, angle_fin_loc);
- circle_2D(X_local, Y_local, rayon_loc, angle_deb_loc, angle_fin_loc);
+ circle_2D(X_local, Y_local, radius_loc, angle_deb_loc, angle_fin_loc);
i_instr=i_instr+6;
break;
- case 5 : // fin
- fin_lettre=1;
- X_print=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
- //cons_servo_main=servo_stylo_lev;
- set_poignet(2);
+ case 5 : // end
+ s_end_char=1;
+ X_print=X_print+size*(font[charact_loc][i_instr+1]);
+ //wrist_servo_setpoint=raised_pen_servopos;
+ set_wrist(2);
//wait_ms(50);
break;
case 6 : // point
- X_local=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
- Y_local=Y_print+taille*(police_maj[lettre_loc][i_instr+2]);
- set_poignet(2);
+ X_local=X_print+size*(font[charact_loc][i_instr+1]);
+ Y_local=Y_print+size*(font[charact_loc][i_instr+2]);
+ set_wrist(2);
goto_2D_line(X_local, Y_local);
- set_poignet(1);
+ set_wrist(1);
goto_2D_line(X_local, Y_local);
- set_poignet(2);
+ set_wrist(2);
i_instr=i_instr+3;
break;
}
}
}
-void print_lettre_fictif(char lettre, float taille)
+void scr_print_single_char_fictive(char charact, float size)
{
- int fin_lettre=0;
+ int s_end_char=0;
int i_instr=0;
- int x_temp, y_temp, lettre_loc;
+ int x_temp, y_temp, charact_loc;
float X_local, Y_local;
- float rayon_loc, angle_deb_loc, angle_fin_loc;
- lettre_loc=lettre;
- //printf ("char: %d", lettre+32);
- if (lettre_loc<0) {
- lettre_loc=lettre+256;
+ float radius_loc, angle_deb_loc, angle_fin_loc;
+ charact_loc=charact;
+ //printf ("char: %d", charact+32);
+ if (charact_loc<0) {
+ charact_loc=charact+256;
}
- //printf ("char fictif: %d\r\n", lettre_loc+32);
- while (fin_lettre==0) {
- switch (police_maj[lettre_loc][i_instr]) {
- case 2 : // aller stylo leve
+ //printf ("char fictive: %d\r\n", charact_loc+32);
+ while (s_end_char==0) {
+ switch (font[charact_loc][i_instr]) {
+ case 2 : // goto pen raised
i_instr=i_instr+3;
break;
- case 3 : // ligne stylo pose
+ case 3 : // line pen pose
i_instr=i_instr+3;
break;
- case 4 : // cercle
+ case 4 : // circle
i_instr=i_instr+6;
break;
- case 5 : // fin
- fin_lettre=1;
- X_fictif=X_fictif+taille*(police_maj[lettre_loc][i_instr+1]);
+ case 5 : // end
+ s_end_char=1;
+ X_fictive=X_fictive+size*(font[charact_loc][i_instr+1]);
break;
case 6 : // point
i_instr=i_instr+3;
@@ -709,19 +681,17 @@
}
}
-void efface(float Xl1, float Yl1, float Xl2, float Yl2)
+void erase(float Xl1, float Yl1, float Xl2, float Yl2)
{
float distance_Y,pas, Y_loc;
- int i, nb_pas, outil_old_loc;
- outil_old_loc=outil;
- set_outil(1, Xl1, Yl1);
+ int i, step_nb, tool_old_loc;
+ tool_old_loc=tool;
+ set_tool(1, Xl1, Yl1);
goto_2D_line(Xl1,Yl1);
- set_poignet(5);
+ set_wrist(5);
distance_Y=Yl2-Yl1;
- //nb_pas=ceil(distance_Y/5);
- //pas=distance_Y/nb_pas;
Y_loc=Yl1;
- //for (i=0;i<=nb_pas;i++)
+
while (Y_loc<Yl2) {
goto_2D_line(Xl1, Y_loc);
goto_2D_line(Xl2, Y_loc);
@@ -730,17 +700,16 @@
goto_2D_line(Xl1, Y_loc);
Y_loc=Y_loc+5;
}
- set_poignet(4);
- //set_outil(outil_old_loc);
+ set_wrist(4);
}
-void print_heure(void)
+void scrib_print_hour(void)
{
char texte_heure[10];
char texte_minute[10];
- int outil_bk,precis_bk, difference;
- int cons_servo_main_bk;
+ int tool_bk,accurate_bk, difference;
+ int wrist_servo_setpoint_bk;
float X_backup, Y_backup,X_pos_bk, Y_pos_bk;
struct tm * timeinfo;
time_t seconds = time(NULL);
@@ -751,266 +720,251 @@
Y_backup=Y_print;
X_pos_bk=X_pos;
Y_pos_bk=Y_pos;
- outil_bk=outil;
- precis_bk=precis;
+ tool_bk=tool;
+ accurate_bk=accurate;
difference=0;
- cons_servo_main_bk=cons_servo_main;
- if (strcmp(texte_minute, texte_minute_bk)!=0) {
- efface(325,107,359,138);
+ wrist_servo_setpoint_bk=wrist_servo_setpoint;
+ if (strcmp(texte_minute, minute_txt_back)!=0) {
+ erase(325,107,359,138);
X_print=330;
Y_print=110;
- print_lettre(texte_minute[0]-32,taille_glob);
+ scr_print_single_char(texte_minute[0]-32,glob_size);
X_print=345;
- print_lettre(texte_minute[1]-32,taille_glob);
- strcpy(texte_minute_bk,texte_minute);
+ scr_print_single_char(texte_minute[1]-32,glob_size);
+ strcpy(minute_txt_back,texte_minute);
difference=1;
}
- if (strcmp(texte_heure, texte_heure_bk)!=0) {
- efface(267,107,304,135);
+ if (strcmp(texte_heure, hour_txt_back)!=0) {
+ erase(267,107,304,135);
X_print=275;
Y_print=110;
- print_lettre(texte_heure[0]-32,taille_glob);
+ scr_print_single_char(texte_heure[0]-32,glob_size);
X_print=290;
- print_lettre(texte_heure[1]-32,taille_glob);
- strcpy(texte_heure_bk,texte_heure);
+ scr_print_single_char(texte_heure[1]-32,glob_size);
+ strcpy(hour_txt_back,texte_heure);
difference=1;
}
- if ((outil_bk==1)&&(difference==1)) {
- set_outil(1,X_pos_bk,Y_pos_bk);
- //goto_2D_short(X_pos_bk,Y_pos_bk,2000);
+ if ((tool_bk==1)&&(difference==1)) {
+ set_tool(1,X_pos_bk,Y_pos_bk);
}
X_print=X_backup;
Y_print=Y_backup;
- //X_pos=X_pos_bk;
- //Y_pos=Y_pos_bk;
- cons_servo_main=cons_servo_main_bk;
- precis=precis_bk;
+
+ wrist_servo_setpoint=wrist_servo_setpoint_bk;
+ accurate=accurate_bk;
}
-void efface_ligne (int line_nb)
+void erase_line (int line_nb)
{
float i;
- //set_outil(1);
+ //set_tool(1);
switch (line_nb) {
case 1 :
- set_outil(1,0,112);
- //goto_2D_line(0,104);
- //wait(2);
- set_poignet(5);
+ set_tool(1,0,112);
+
+ set_wrist(5);
for (i=0; i<31; i=i+10.6) {
goto_2D_line(0,112+i);//104
- //print_heure();
+ //scrib_print_hour();
goto_2D_line(120,116+i);//113
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,109+i);//104
- print_heure();
- //goto_2D_line(360,107+i);
- //goto_2D_line(360,107+i+5);
+ scrib_print_hour();
+
goto_2D_line(265,108+i);
- print_heure();
+ scrib_print_hour();
goto_2D_line(265,108+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,109+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(120,116+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(0,112+i+5.3);
- //print_heure();
+ //scrib_print_hour();
}
break;
case 2 :
- set_outil(1,0,70);
- //goto_2D_line(0,70);
- //wait(2);
- set_poignet(5);
+ set_tool(1,0,70);
+
+ set_wrist(5);
for (i=0; i<31; i=i+10.6) { //8
goto_2D_line(0,76+i);//104
- //print_heure();
+ //scrib_print_hour();
goto_2D_line(120,81+i);//113
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,78+i);//104
- print_heure();
+ scrib_print_hour();
goto_2D_line(360,72+i);
- print_heure();
+ scrib_print_hour();
goto_2D_line(360,72+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,78+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(120,81+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(0,76+i+5.3);//4
- //print_heure();
+ //scrib_print_hour();
}
break;
case 3 :
- //goto_2D_line(0,37);
- set_outil(1,0,37);
- //wait(2);
- set_poignet(5);
- cons_servo_main=745;
+
+ set_tool(1,0,37);
+
+ set_wrist(5);
+ wrist_servo_setpoint=745;
for (i=0; i<31; i=i+10.6) {
goto_2D_line(0,39+i);//104
- //print_heure();
+ //scrib_print_hour();
goto_2D_line(120,46+i);//113
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,47+i);//104
- print_heure();
+ scrib_print_hour();
goto_2D_line(360,44+i);
- print_heure();
+ scrib_print_hour();
goto_2D_line(360,44+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,47+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(120,46+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(0,39+i+5.3);
}
break;
case 4 :
- //goto_2D_line(0,5);
- set_outil(1,0,5);
- //wait(2);
- set_poignet(5);
- cons_servo_main=740;
+
+ set_tool(1,0,5);
+
+ set_wrist(5);
+ wrist_servo_setpoint=740;
for (i=0; i<31; i=i+10.6) {
goto_2D_line(0,5+i);//104
- //print_heure();
+ //scrib_print_hour();
goto_2D_line(120,12+i);//113
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,12+i);//104
- print_heure();
+ scrib_print_hour();
goto_2D_line(360,8+i);
- print_heure();
+ scrib_print_hour();
goto_2D_line(360,8+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(240,12+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(120,12+i+5.3);
- print_heure();
+ scrib_print_hour();
goto_2D_line(0,5+i+5.3);
}
break;
}
- set_poignet(4);
+ set_wrist(4);
- //set_outil(0);
}
-void retour_ligne(void)
+void scrib_line_return(void)
{
- switch (ligne) {
+ switch (line) {
case 1:
- remplissage[1]=1;
- if (remplissage[2]==1) {
- efface_ligne(2);
+ line_filling_st[1]=1;
+ if (line_filling_st[2]==1) {
+ erase_line(2);
}
X_print=1;
Y_print=77;
- ligne=2;
+ line=2;
break;
case 2:
- remplissage[2]=1;
- if (remplissage[3]==1) {
- efface_ligne(3);
+ line_filling_st[2]=1;
+ if (line_filling_st[3]==1) {
+ erase_line(3);
}
X_print=1;
Y_print=44;
- ligne=3;
+ line=3;
break;
case 3:
- remplissage[3]=1;
- if (remplissage[4]==1) {
- efface_ligne(4);
+ line_filling_st[3]=1;
+ if (line_filling_st[4]==1) {
+ erase_line(4);
}
X_print=1;
Y_print=11;
- ligne=4;
+ line=4;
break;
case 4:
- remplissage[4]=1;
- if (remplissage[1]==1) {
- efface_ligne(1);
+ line_filling_st[4]=1;
+ if (line_filling_st[1]==1) {
+ erase_line(1);
}
X_print=1;
Y_print=110;
- ligne=1;
+ line=1;
break;
}
}
-void print_heure_initial(void)
+void scrib_print_hour_initial(void)
{
char texte_heure[10];
- //float X_backup, Y_backup;
struct tm * timeinfo;
time_t seconds = time(NULL);
timeinfo = localtime (&seconds);
- strftime(texte_heure_bk,10,"%H",timeinfo);
- strftime(texte_minute_bk,10,"%M",timeinfo);
+ strftime(hour_txt_back,10,"%H",timeinfo);
+ strftime(minute_txt_back,10,"%M",timeinfo);
X_print=275;
Y_print=110;
- print_lettre(texte_heure_bk[0]-32,taille_glob);
+ scr_print_single_char(hour_txt_back[0]-32,glob_size);
X_print=290;
- print_lettre(texte_heure_bk[1]-32,taille_glob);
+ scr_print_single_char(hour_txt_back[1]-32,glob_size);
X_print=310;
- print_lettre(104-32,taille_glob);
+ scr_print_single_char(104-32,glob_size);
X_print=330;
- print_lettre(texte_minute_bk[0]-32,taille_glob);
+ scr_print_single_char(minute_txt_back[0]-32,glob_size);
X_print=345;
- print_lettre(texte_minute_bk[1]-32,taille_glob);
+ scr_print_single_char(minute_txt_back[1]-32,glob_size);
}
-void print_chaine(char* chaine)
+void scrib_print_string(char* chaine)
{
int i=0;
- int i_fictif=0;
- int retour_ligne_loc=0;
+ int i_fictive=0;
+ int line_feed_loc=0;
int rupt=0;
while (chaine[i]!=0) {
- print_heure();
- retour_ligne_loc=0;
- //printf("interp: %d\r\n",chaine[i]);
+ scrib_print_hour();
+ line_feed_loc=0;
if (chaine[i]>31) {
- print_lettre(chaine[i]-32,taille_glob);
+ scr_print_single_char(chaine[i]-32,glob_size);
} else if (chaine[i]==10) {
- retour_ligne();
- //X_print=1;
- //Y_print=Y_print-22*taille_glob;
+ scrib_line_return();
+
}
- if ((X_print>345)||((ligne==1)&&(X_print>270))) {
- retour_ligne();
- //X_print=1;
- //Y_print=Y_print-22*taille_glob;
- retour_ligne_loc=1;
+ if ((X_print>345)||((line==1)&&(X_print>270))) {
+ scrib_line_return();
+
+ line_feed_loc=1;
}
- //i++;
- if ((chaine[i]==32)&&(retour_ligne_loc==0)) { //determine si retour a la ligne suite a espace
- X_fictif=X_print;
- i_fictif=i+1;
+ if ((chaine[i]==32)&&(line_feed_loc==0)) { //determine
+ X_fictive=X_print;
+ i_fictive=i+1;
rupt=0;
- while (rupt==0) { //on fait avancer X_fictif jusqu'au prochain caractère espace ou retour ou fin
- if (chaine[i_fictif]>31) {
- print_lettre_fictif(chaine[i_fictif]-32,taille_glob);
+ while (rupt==0) { //we step X_fictive until the next space char or line feed or endofchar.
+ if (chaine[i_fictive]>31) {
+ scr_print_single_char_fictive(chaine[i_fictive]-32,glob_size);
}
- //pc.printf("Xfictif %f\r\n",X_fictif);
- if ((X_fictif>345)||((ligne==1)&&(X_fictif>270))) {
+ if ((X_fictive>345)||((line==1)&&(X_fictive>270))) {
rupt=2;
}
- if ((chaine[i_fictif]==32)||(chaine[i_fictif]==10)||(chaine[i_fictif]==0)) {
+ if ((chaine[i_fictive]==32)||(chaine[i_fictive]==10)||(chaine[i_fictive]==0)) {
rupt=1;
- //pc.printf("rupt 1 %d\r\n",chaine[i_fictif]);
}
- i_fictif++;
+ i_fictive++;
}
if (rupt==2) {
- retour_ligne();
- //X_print=1;
- //Y_print=Y_print-22*taille_glob;
- //pc.printf("rupt 2\r\n");
+ scrib_line_return();
+
}
}
i++;
@@ -1031,48 +985,23 @@
struct tm * timeinfo;
time_t seconds = time(NULL);
- // ********* initialisations *******
- //while(1){}
+ // ********* initializations *******
+
pc.baud(115200);
pc.printf("bijour\r\n");
- tick_attached_coude=0;
- tick_attached_epaule=0;
+ tick_attached_elbow=0;
+ tick_attached_shoulder=0;
X_print=1;
Y_print=110; //110
- ligne=1;
- precis=0;
- outil=0;
- taille_glob=1.5;
- appel_servo.attach_us(&tick_servo, 20000);
- //myled=1;
- //wait(1);
- //myled=0;
- //while(1) {
- // aaa=potard_1.read();
- // i=switch_epaule.read();
- // j=switch_coude.read();
- // pc.printf("S1:%i S2:%i potard:%f\r\n", i,j,aaa);
- // wait(1);
- //}
+ line=1;
+ accurate=0;
+ tool=0;
+ glob_size=1.5;
+ servo_call.attach_us(&tick_servo, 20000);
- //for (i=1;i<150;i++) {
- // pc.printf("i= %d",i);
- // stepper_clk_1=1;
- // stepper_clk_2=1;
- // wait_us(10);
- // stepper_clk_1=0;
- // stepper_clk_2=0;
- // wait_ms(1000);
- //}
- cons_servo_main=servo_neutre;
+ wrist_servo_setpoint=neutral_pen_servopos;
wait(1);
- //while(1) {
- // cons_servo_main=1800;
- // wait(3);
- // cons_servo_main=1200;
- // wait(3);
- // }
fileid=fopen("/local/input.txt", "r");
fread(montexte,1,500,fileid);
@@ -1087,248 +1016,34 @@
//set_time(1503151338);//set_time(1336478783);
timeinfo = localtime (&seconds);
- //strftime(montexte,10,"%Hh%M",timeinfo);
- //pc.printf(montexte);
init_stepper();
- //setpos_ang (0, 0, 3000);
- //tick_detach();
- //set_poignet(1);
- //wait(5);
- //set_poignet(3);
- //setpos_ang (-850, 0, 3000);
- //tick_detach();
- //set_poignet(5);
- //wait(5);
- //set_poignet(3);
- //goto_2D_short(0,0,3000);
- //set_poignet(1);
- //goto_2D_line(360,0);
- //goto_2D_line(360,140);
- //goto_2D_line(0,140);
- //goto_2D_line(0,0);
- //wait(2);
- //set_outil(1);
- //goto_2D_line(0,0);
- //set_poignet(5);
- //goto_2D_line(360,0);
- //goto_2D_line(360,140);
- //goto_2D_line(0,140);
- //goto_2D_line(0,0);
- //set_outil(0);
- //printf(montexte);
+
+
- //wait (4);
myled=!myled;
- //setpos_ang (0, 0, 3000);
- //tick_detach();
- //set_poignet(1);
- //wait(20);
+
goto_2D_short (0,110, 1000);
tick_detach();
- //goto_2D_line(0,134);
- //set_poignet(1);
- //goto_2D_line(360,134);
- //set_poignet(2);
- //goto_2D_line(360,104);
- //set_poignet(1);
- //goto_2D_line(0,104);
- //set_poignet(2);
- //goto_2D_line(0,101);
- //set_poignet(1);
- //goto_2D_line(360,101);
- //set_poignet(2);
- //goto_2D_line(360,71);
- //set_poignet(1);
- //goto_2D_line(0,71);
- //set_poignet(2);
- //goto_2D_line(0,68);
- //set_poignet(1);
- //goto_2D_line(360,68);
- //set_poignet(2);
- //goto_2D_line(360,38);
- //set_poignet(1);
- //goto_2D_line(0,38);
- //set_poignet(2);
- //goto_2D_line(0,35);
- //set_poignet(1);
- //goto_2D_line(360,35);
- //set_poignet(2);
- //goto_2D_line(360,5);
- //set_poignet(1);
- //goto_2D_line(0,5);
- //set_poignet(2);
- //wait(1);
- //efface_ligne(4);
- //setpos_ang (-850, 0, 1000);
- //setpos_ang (0, 0, 3000);
- //tick_detach();
- //wait(10);
- //cons_servo_main=servo_gomme_pos;
- //print_chaine(ctime(&seconds));
+
- print_heure_initial();
+ scrib_print_hour_initial();
X_print=1;
Y_print=110;//110
- //taille_glob=3;
- //print_chaine("ROB");
-
- //circle_2D(108,68,8,-1.57,-3.14);//98
- //goto_2D_line(100,100);//90
- //circle_2D(108,100,8,3.14,1.57);//98
- //goto_2D_line(140,108);//130
- //circle_2D(140,100,8,1.57,0);//130
- //goto_2D_line(148,68);//138
- //circle_2D(140,68,8,0,-1.57);//130
- //goto_2D_line(108,60);//98
- //set_poignet(2);
-
- //circle_2D (114,88,6,-3.14,3.14);//104
- //circle_2D (134,88,6,-3.14,3.14);//124
- //circle_2D(96,84,15,1.57,4.71);//86
- //goto_2D_line(96,99);//86
- //set_poignet(2);
-
- //circle_2D(152,84,15,1.57,-1.57);//142
- //goto_2D_line(152,99);//142
- //set_poignet(2);
- //circle_2D(124,108,15,3.14,0);//114
- //set_poignet(2);
- //goto_2D_line(124,123);//114
- //set_poignet(1);
- //goto_2D_line(124,135);//114
- //set_poignet(2);
- //circle_2D(124,140,5,-3.14,3.14);//114
-
- //X_print=165;
- //print_chaine("T");
- //taille_glob=1.5;
- //X_print=20;
- //Y_print=30;
- //print_chaine("- MAKER.COM");
- //set_poignet(2);
- //goto_2D_line(350,130);
- //set_poignet(3);
- //while(1) {}
while(1) {
- //print_chaine("Bonjour a tous!");
- print_chaine(montexte);
- //set_outil(1);
- //wait(2);
- //set_outil(0);
- //efface(1,20,300,44);//110 134
- }
- while(1) {}
- while(1) {
- for (i=26; i<40; i++) {
- print_lettre (i,2);
- }
- X_print=20;
- Y_print=30;
- goto_2D_line(20,40);
- for (i=40; i<53; i++) {
- print_lettre (i,2);
- }
- goto_2D_line(350,100);
- //print_lettre(1, 2);
- //print_lettre(2, 2);
- //print_lettre(3, 2);
- //print_lettre(4, 2);
- //print_lettre(5, 2);
- //print_lettre(6, 2);
- //X_print=10;
- //Y_print=10;
- //print_lettre(1, 2);
- //print_lettre(2, 2);
- //print_lettre(3, 2);
- //print_lettre(4, 2);
- //print_lettre(5, 2);
- //print_lettre(6, 2);
- wait(15);
- X_print=1;
- Y_print=60;
- }
- //cons_servo_main=1800;//1800
- set_poignet(3);
-
- while (1) {
-
- wait(1);
+ scrib_print_string(montexte);
- goto_2D_line(50,10);
- goto_2D_line(50,70);
- circle_2D(50, 55, 15, 1.57, -1.57);
- circle_2D(50, 25, 15, 1.57, -1.57);
- wait(2);
- goto_2D_line(360,0);
- myled=!myled;
- goto_2D_line(360,110);
- myled=!myled;
- goto_2D_line(0,110);
- myled=!myled;
- goto_2D_line(0,0);//
- myled=!myled;
- //goto_2D_line(180,0);
- //myled=!myled;
- //goto_2D_line(180,110);
- //myled=!myled;
- //goto_2D_line(360,110);
- //myled=!myled;
- //goto_2D_line(360,0);
- //myled=!myled;
- //goto_2D_line(180,0);
- //myled=!myled;
- //goto_2D_line(180,110);
- //myled=!myled;
- //goto_2D_line(0,110);
- //myled=!myled;
- //goto_2D_line(0,0);
- circle_2D(180, 55, 55, 0, 4.7);
- circle_2D(180, 55, 20, -1.57, 1.57);
- circle_2D(210, 55, 20, -1.57, 1.57);
- circle_2D(250, 55, 20, -1.57, 1.57);
- circle_2D(290, 55, 20, -1.57, 1.57);
-
- //myled=!myled;
- //goto_2D_short (180,0, 1000);
- //wait(2);
- //myled=!myled;
- //goto_2D_short (360,0, 1000);
- //wait(2);
- //myled=!myled;
- //goto_2D_short (360,110, 1000);
- //wait(2);
- //myled=!myled;
- //goto_2D_short (180,110, 1000);
- //wait(2);
- //myled=!myled;
- //goto_2D_short (0,110, 1000);
- //wait(2)
- //goto_2D_short (360,110);
}
while(1) {}
- a_coude=0;
- a_epaule=0;
+ set_wrist(3);
+
+ while(1) {}
+
+ a_elbow=0;
+ a_shoulder=0;
aaa=1000;
bbb=0;
- while (1) {
- myled=!myled;
- setpos_ang (aaa,bbb, 2000);
- myled=!myled;
- setpos_ang (aaa,aaa, 2000);
- myled=!myled;
- setpos_ang (bbb,aaa, 500);
- myled=!myled;
- setpos_ang (bbb,bbb, 500);
- }
-
- while(1) {
- myled = 1;
- wait(0.2);
- myled = 0;
- wait(0.2);
- }
}