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:
- 0:694620b4c620
- Child:
- 1:163fa4a925d9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Apr 22 13:57:53 2018 +0000
@@ -0,0 +1,1334 @@
+#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
+
+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 servoout(p17);
+AnalogIn potard_1 (p20);
+DigitalIn switch_epaule(p18);
+DigitalIn switch_coude(p19);
+Serial pc(USBTX, USBRX);
+LocalFileSystem local("local");
+
+Timeout pulse_stepper1;
+Timeout pulse_stepper2;
+Timeout servo_timeout;
+Ticker appel_servo;
+
+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;
+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 taille_glob;
+
+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
+ ,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
+ ,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,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 /
+ ,4,5,11,8,16,8,3,9,5,4,5,5,8,8,0,3,1,11,5,10 ,0,0,0,0,0,0,0,0,0,0 //48 0
+ ,2,1,12,3,4,15,3,4,1,2,2,1,3,6,1,5,7,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //49 1
+ ,4,5,11,8,16,6,3,1,1,3,9,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //50 2
+ ,4,5,12,6,15,4,4,5,5,8,12,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //51 3
+ ,2,9,7,3,1,7,3,7,15,3,7,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //52 4
+ ,2,8,15,3,2,15,3,1,9,3,4,9,4,4,5,8,12,2,5,9 ,0,0,0,0,0,0,0,0,0,0 //53 5
+ ,4,5,5,8,16,0,3,1,8,4,8,8,14,16,12,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //54 6
+ ,2,1,15,3,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 //55 7
+ ,4,5,12,6,4,20,4,5,5,8,28,12,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //56 8
+ ,4,5,11,8,24,8,3,9,8,4,2,8,14,8,4,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //57 9
+ ,6,2,7,6,2,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 //58 :
+ ,6,3,7,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 //59 ;
+ ,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 //60 <
+ ,2,2,7,3,8,7,2,2,3,3,8,3,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //61 =
+ ,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 //62 >
+ ,4,5,11,8,16,8,4,7,11,4,8,4,4,7,7,4,12,16,3,5 ,4,6,5,1,5,10,0,0,0,0 //63 ?
+ ,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 //64
+
+ ,2,1,1,3,6,15,3,11,1,2,3,6,3,9,6,5,12,0,0,0,0,0,0,0,0,0,0,0,0,0 //A 65
+ ,2,1,1,3,1,15,3,3,15,4,3,12,6,12,4,3,1,9,4,3,5,8,12,4,3,1,1,5,8,0 //B 66
+ ,4,5,5,8,7,0,3,1,11,4,5,11,8,16,9,5,10,0,0,0,0,0,0,0,0,0,0,0,0,0 //C 67
+ ,2,1,1,3,1,15,3,4,15,4,4,11,8,12,8,3,8,5,4,4,5,8,8,4,3,1,1,5,9,0 //D 68
+ ,2,8,15,3,1,15,3,1,1,3,8,1,2,1,8,3,6,8,5,9 ,0,0,0,0,0,0,0,0,0,0 //E 69
+ ,2,8,15,3,1,15,3,1,1,2,1,8,3,6,8,5,9,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //F 70
+ ,4,5,11,8,9,16,3,1,5,4,5,5,8,0,8,3,9,8,3,5 ,8,5,10,0,0,0,0,0,0,0 //G 71
+ ,2,1,15,3,1,1,2,1,8,3,8,8,2,8,15,3,8,1,5,9 ,0,0,0,0,0,0,0,0,0,0 //H 72
+ ,2,1,15,3,5,15,2,3,15,3,3,1,2,1,1,3,5,1,5,6 ,0,0,0,0,0,0,0,0,0,0 //I 73
+ ,4,4,4,6,0,8,3,7,15,2,5,15,3,9,15,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //J 74
+ ,2,1,1,3,1,15,2,7,15,3,1,8,3,7,1,5,8,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //K 75
+ ,2,1,15,3,1,1,3,8,1,5,9,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //L 76
+ ,2,1,1,3,1,15,3,5,8,3,9,15,3,9,1,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //M 77
+ ,2,1,1,3,1,15,3,9,1,3,9,15,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //N 78
+ ,4,5,11,8,16,8,3,9,5,4,5,5,8,8,0,3,1,11,5,10 ,0,0,0,0,0,0,0,0,0,0 //O 79
+ ,2,1,1,3,1,15,3,3,15,4,3,12,6,12,4,3,1,9,5,7 ,0,0,0,0,0,0,0,0,0,0 //P 80
+ ,4,5,11,8,16,8,3,9,5,4,5,5,8,8,0,3,1,11,2,6,4,3,9,1,5,10,0,0,0,0 //Q 81
+ ,2,1,1,3,1,15,3,3,15,4,3,12,6,12,4,3,1,9,2,3,9,3,6,1,5,7,0,0,0,0 //R 82
+ ,4,5,11,8,8,16,3,9,5,4,5,5,8,8,0,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //S 83
+ ,2,5,1,3,5,15,2,1,15,3,9,15,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //T 84
+ ,2,1,15,3,1,5,4,5,5,8,0,8,3,9,15,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //U 85
+ ,2,1,15,3,5,1,3,9,15,5,10,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //V 86
+ ,2,1,15,3,5,1,3,7,8,3,9,1,3,13,15,5,14,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //W 87
+ ,2,9,15,3,1,1,2,1,15,3,9,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //X 88
+ ,2,9,15,3,1,1,2,1,15,3,5,8,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //Y 89
+ ,2,1,15,3,9,15,3,1,1,3,9,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //Z 90
+
+ ,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 //91
+ ,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 //92
+ ,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 //93
+ ,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 //94
+ ,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 //95
+ ,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 //96
+
+ ,4,4,4,6,8,24,3,7,7,4,4,7,6,8,15,5,8,0,0,0 ,0,0,0,0,0,0,0,0,0,0 // a 27
+ ,4,5,5,8,0,16,2,1,1,3,1,15,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //b 28
+ ,4,5,5,8,10,22,5,9,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //c 29
+ ,4,5,5,8,24,8,2,9,1,3,9,15,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //d 30
+ ,2,1,5,3,9,5,4,5,5,8,8,22,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //e 31
+ ,2,1,1,3,1,10,4,4,10,6,16,10,2,4,7,3,1,7,5,7,0,0,0,0,0,0,0,0,0,0 //f 32
+ ,4,5,5,8,24,8,3,9,0,4,5,0,8,8,1,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //g 33
+ ,2,1,15,3,1,1,4,5,5,8,16,8,3,9,1,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //h 34
+ ,2,2,1,3,2,9,6,2,12,5,4,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //i 35
+ ,6,6,12,2,6,9,3,6,-1,4,3,-1,6,8,0,5,7,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //j 36
+ ,2,1,15,3,1,1,2,6,9,3,1,5,3,6,1,5,7,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //k 37
+ ,2,2,15,3,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 //l 38
+ ,2,1,9,3,1,1,4,4,6,6,16,8,3,7,1,4,10,6,6,16 ,8,3,13,1,5,14,0,0,0,0 //m 39
+ ,2,1,9,3,1,1,4,5,5,8,16,8,3,9,1,5,10,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //n 40
+ ,4,5,5,8,0,16,5,10,0,0,0,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //o 41
+ ,2,1,-3,3,1,9,4,5,5,8,0,16,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //p 42
+ ,4,5,5,8,0,16,2,9,9,3,9,-3,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //q 43
+ ,2,1,9,3,1,1,4,5,5,8,16,9,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //r 44
+ ,2,7,9,3,3,9,4,3,7,4,12,20,3,5,5,4,5,3,4,12 ,4,3,1,1,5,8,0,0,0,0 //s 45
+ ,2,1,15,3,1,4,4,4,4,6,0,8,2,1,11,3,6,11,5,8 ,0,0,0,0,0,0,0,0,0,0 //t 46
+ ,2,1,9,3,1,4,4,4,4,6,0,8,3,7,9,5,8,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //u 47
+ ,2,1,9,3,5,1,3,9,9,5,10,0,0,0,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //v 48
+ ,2,1,9,3,4,1,3,6,5,3,8,1,3,11,9,5,12,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //w 49
+ ,2,7,9,3,1,1,2,1,9,3,7,1,5,8,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //x 50
+ ,2,1,9,3,5,1,2,9,9,3,2,-5,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //y 121
+ ,2,1,9,3,9,9,3,1,1,3,9,1,5,10,0,0,0,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //z 122
+
+ ,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 //123
+ ,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 //124
+ ,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 //125
+ ,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 //126
+ ,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 //127
+ ,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 //128
+ ,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 //129
+ ,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 //130
+ ,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 //131
+ ,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 //132
+ ,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 //133
+ ,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 //134
+ ,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 //135
+ ,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 //136
+ ,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 //137
+ ,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 //138
+ ,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 //139
+ ,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 //140
+ ,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 //141
+ ,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 //142
+ ,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 //143
+ ,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 //144
+ ,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 //145
+ ,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 //146
+ ,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 //147
+ ,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 //148
+ ,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 //149
+ ,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 //150
+ ,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 //151
+ ,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 //152
+ ,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 //153
+ ,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 //154
+ ,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 //155
+ ,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 //156
+ ,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 //157
+ ,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 //158
+ ,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 //159
+ ,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 //160
+ ,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 //161
+ ,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 //162
+ ,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 //163
+ ,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 //164
+ ,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 //165
+ ,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 //166
+ ,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 //167
+ ,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 //168
+ ,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 //169
+ ,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 //170
+ ,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 //171
+ ,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 //172
+ ,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 //173
+ ,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 //174
+ ,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 //175
+ ,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 //176
+ ,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 //177 +/-
+ ,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 //178
+ ,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 //179
+ ,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 //180
+ ,2,1,-3,3,1,9,4,4,4,6,0,8,3,7,9,5,8,0,0,0 ,0,0,0,0,0,0,0,0,0,0 //181 µ
+ ,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 //182
+ ,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 //183
+ ,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 //184
+ ,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 //185
+ ,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 //186
+ ,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 //187
+ ,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 //188
+ ,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 //189
+ ,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 //190
+ ,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 //191
+ ,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 //192
+ ,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 //193
+ ,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 //194
+ ,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 //195
+ ,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 //196
+ ,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 //197
+ ,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 //198
+ ,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 //199
+ ,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 //200
+ ,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 //201
+ ,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 //202
+ ,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 //203
+ ,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 //204
+ ,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 //205
+ ,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 //206
+ ,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 //207
+ ,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 //208
+ ,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 //209
+ ,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 //210
+ ,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 //211
+ ,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 //212
+ ,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 //213
+ ,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 //214
+ ,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 //215
+ ,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 //216
+ ,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 //217
+ ,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 //218
+ ,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 //219
+ ,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 //220
+ ,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 //221
+ ,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 //222
+ ,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 //223
+ ,4,4,4,6,8,24,3,7,7,4,4,7,6,8,15,2,2,14,3,6 ,12,5,8,0,0,0,0,0,0,0 //224 à
+ ,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 //225
+ ,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 //226
+ ,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 //227
+ ,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 //228
+ ,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 //229
+ ,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 //230
+ ,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 //231
+ ,2,1,5,3,9,5,4,5,5,8,8,22,2,3,14,3,7,12,5,10 ,0,0,0,0,0,0,0,0,0,0 //232 è
+ ,2,1,5,3,9,5,4,5,5,8,8,22,2,7,14,3,3,12,5,10 ,0,0,0,0,0,0,0,0,0,0 //233 é
+ ,2,1,5,3,9,5,4,5,5,8,8,22,2,2,12,3,5,14,3,8 ,12,5,10,0,0,0,0,0,0,0//234 ê
+ ,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 //235
+ ,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 //236
+ ,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 //237
+ ,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 //238
+ ,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 //239 ï
+ ,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 //240
+ ,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 //241
+ ,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 //242
+ ,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 //243
+ ,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 //244 ô
+ ,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 //245
+ ,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 //246 ö
+ ,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 //247
+ ,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 //248
+ ,2,1,9,3,1,4,4,4,4,6,0,8,3,7,9,2,2,14,3,6,12,5,8,0,0,0,0,0,0,0 //249 ù
+ ,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 //250
+ ,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 //251
+};
+
+void stop_tick_servo(void)
+{
+ servoout=0;
+}
+
+void tick_servo (void)
+{
+ servoout=1;
+ servo_timeout.attach_us(&stop_tick_servo, cons_servo_main);
+}
+
+void clk_stepper1(void)
+{
+ stepper_clk_1=1;
+ pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
+ a_coude=a_coude + dir_1_real;
+ stepper_clk_1=0;
+}
+
+void clk_stepper2(void)
+{
+ stepper_clk_2=1;
+ pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
+ a_epaule=a_epaule + dir_2_real;
+ stepper_clk_2=0;
+}
+
+void tick_detach()
+{
+ tick_attached_coude=0;
+ tick_attached_epaule=0;
+ pulse_stepper1.detach();
+ pulse_stepper2.detach();
+}
+
+void set_poignet(int pospoignet)
+{
+ switch (pospoignet) {
+ case 1 : //stylo pose
+ if (cons_servo_main!=servo_stylo_pos) {
+ wait_ms(100);
+ cons_servo_main=servo_stylo_pos;
+ myled=1;
+ wait_ms(80);
+ }
+ precis=1;
+ break;
+ case 2 : //stylo leve
+ if (cons_servo_main!=servo_stylo_lev) {
+ //wait_ms(50);
+ cons_servo_main=servo_stylo_lev;
+ myled=0;
+ wait_ms(50);
+ }
+ precis=0;
+ break;
+ case 3 : //neutre
+ cons_servo_main=servo_neutre;
+ precis=0;
+ break;
+ case 4 : //gomme levee
+ cons_servo_main=servo_gomme_lev;
+ precis=0;
+ break;
+ case 5 : //gomme posee
+ cons_servo_main=servo_gomme_pos;
+ precis=2;
+ }
+}
+
+
+
+void init_stepper()
+{
+ //int arret=0;
+ int i;
+ while (switch_epaule==1) {
+ stepper_clk_2=1;
+ wait_us(10);
+ stepper_clk_2=0;
+ wait_ms(3);
+ }
+ stepper_dir_2=1;
+ for (i=1; i<2620; i++) { //2640
+ stepper_clk_2=1;
+ wait_us(10);
+ stepper_clk_2=0;
+ wait_ms(1);
+ }
+ while (switch_coude==1) {
+ stepper_clk_1=1;
+ wait_us(10);
+ stepper_clk_1=0;
+ wait_ms(3);
+ }
+ a_coude=-2940;//2900
+ a_epaule=0;
+ //stepper_dir_1=1;
+ // for (i=1;i<2900;i++) {
+ // stepper_clk_1=1;
+ // wait_us(10);
+ // stepper_clk_1=0;
+ // wait_ms(1);
+ //}
+ //cons_servo_main=1800;
+}
+
+void setpos_ang (int a_coude_cible, int a_epaule_cible, int delay_ms)
+{
+ int err_a_coude, err_a_epaule;
+ 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) {
+ stepper_dir_1=1;
+ dir_1_real=1;
+ } else {
+ stepper_dir_1=0;
+ dir_1_real=-1;
+ };
+ if (err_a_epaule>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);
+ 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);
+
+ delay_1=250;
+ led_err=1;
+ //wait(1);
+ led_err=0;
+ }
+ period_stepper1=delay_1-3;
+ if (tick_attached_coude==0) {
+ pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
+ tick_attached_coude=1;
+ }
+ } else {
+ tick_attached_coude=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 (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);
+ delay_2=250;
+ led_err=1;
+ //wait(1);
+ led_err=0;
+ }
+ period_stepper2=delay_2-3;
+ if (tick_attached_epaule==0) {
+ pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
+ tick_attached_epaule=1;
+ }
+ } else {
+ tick_attached_epaule=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)
+{
+ float allonge, deltax, deltay, alpha_D, arad_coude_cible, arad_epaule_cible, consigne_coude, consigne_epaule, erreur_coude, erreur_epaule;
+ int delay_loc;
+ // inversion matrice simplifiee
+ if (X_cible > 360) {
+ X_cible=360;
+ }
+ if (X_cible < 0) {
+ X_cible=0;
+ }
+ if (Y_cible > 145) {
+ Y_cible=145; //110
+ }
+ if (Y_cible < 0) {
+ Y_cible=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
+ 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;
+ } 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;
+ }
+
+ //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;
+ } else {
+ delay_loc=0.75*erreur_epaule;
+ }
+ setpos_ang (consigne_coude, consigne_epaule, delay_loc);
+ X_pos=X_cible;
+ Y_pos=Y_cible;
+ } else {
+ setpos_ang (consigne_coude, consigne_epaule, delay_ms);
+ X_pos=X_cible;
+ Y_pos=Y_cible;
+ //cinematique directe:
+ }
+}
+
+void goto_2D_line (float X_cible, float Y_cible)
+{
+ 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;
+ 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;
+ x_decomp=X_pos;
+ y_decomp=Y_pos;
+ //tick_detach();
+ for (i=1; i<=nb_pas; 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) {
+ goto_2D_short (x_decomp, y_decomp, 10+70*potard_1.read()); //12
+ } else if (precis==2) {
+ goto_2D_short (x_decomp, y_decomp,13);
+ } else {
+ goto_2D_short (x_decomp, y_decomp, 9);
+ }
+ }
+ tick_detach();
+}
+
+void circle_2D(float X_centre, float Y_centre, float rayon, 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);
+ 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);
+ 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);
+ 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)
+{
+ if ((outil_loc==1)&&(outil==0)) {//gomme
+ set_poignet(3);
+ outil=1;
+ goto_2D_short (X_loc, Y_loc,0);//2000
+ tick_detach();
+ set_poignet(4);
+ wait_ms(200);
+ } else if ((outil_loc==0)&&(outil==1)) {
+ set_poignet(3);
+ outil=0;
+ goto_2D_short (X_loc, Y_loc,0);//2000
+ tick_detach();
+ set_poignet(2);
+ wait_ms(200);
+ } else if ((outil_loc==1)&&(outil==1)) {
+ set_poignet(4);
+ }
+}
+
+void print_lettre(char lettre, float taille)
+{
+ int fin_lettre=0;
+ int i_instr=0;
+ int x_temp, y_temp, lettre_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;
+ }
+ //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);
+ //wait_ms(100);
+ X_local=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
+ y_temp=police_maj[lettre_loc][i_instr+2];
+ if (y_temp>127) {
+ y_temp=y_temp-256;
+ }
+ Y_local=Y_print+taille*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;
+ // wait_ms(100);
+ //}
+ X_local=X_print+taille*(police_maj[lettre_loc][i_instr+1]);
+ y_temp=police_maj[lettre_loc][i_instr+2];
+ if (y_temp>127) {
+ y_temp=y_temp-256;
+ }
+ Y_local=Y_print+taille*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];
+ 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];
+ 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;
+ //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);
+ 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);
+ //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);
+ goto_2D_line(X_local, Y_local);
+ set_poignet(1);
+ goto_2D_line(X_local, Y_local);
+ set_poignet(2);
+ i_instr=i_instr+3;
+ break;
+ }
+ }
+}
+
+void print_lettre_fictif(char lettre, float taille)
+{
+ int fin_lettre=0;
+ int i_instr=0;
+ int x_temp, y_temp, lettre_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;
+ }
+ //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
+ i_instr=i_instr+3;
+ break;
+ case 3 : // ligne stylo pose
+ i_instr=i_instr+3;
+ break;
+ case 4 : // cercle
+ i_instr=i_instr+6;
+ break;
+ case 5 : // fin
+ fin_lettre=1;
+ X_fictif=X_fictif+taille*(police_maj[lettre_loc][i_instr+1]);
+ break;
+ case 6 : // point
+ i_instr=i_instr+3;
+ break;
+ }
+ }
+}
+
+void efface(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);
+ goto_2D_line(Xl1,Yl1);
+ set_poignet(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);
+ Y_loc=Y_loc+5;
+ goto_2D_line(Xl2, Y_loc);
+ goto_2D_line(Xl1, Y_loc);
+ Y_loc=Y_loc+5;
+ }
+ set_poignet(4);
+ //set_outil(outil_old_loc);
+}
+
+
+void print_heure(void)
+{
+ char texte_heure[10];
+ char texte_minute[10];
+ int outil_bk,precis_bk, difference;
+ int cons_servo_main_bk;
+ float X_backup, Y_backup,X_pos_bk, Y_pos_bk;
+ struct tm * timeinfo;
+ time_t seconds = time(NULL);
+ timeinfo = localtime (&seconds);
+ strftime(texte_heure,10,"%H",timeinfo);
+ strftime(texte_minute,10,"%M",timeinfo);
+ X_backup=X_print;
+ Y_backup=Y_print;
+ X_pos_bk=X_pos;
+ Y_pos_bk=Y_pos;
+ outil_bk=outil;
+ precis_bk=precis;
+ difference=0;
+ cons_servo_main_bk=cons_servo_main;
+ if (strcmp(texte_minute, texte_minute_bk)!=0) {
+ efface(325,107,359,138);
+ X_print=330;
+ Y_print=110;
+ print_lettre(texte_minute[0]-32,taille_glob);
+ X_print=345;
+ print_lettre(texte_minute[1]-32,taille_glob);
+ strcpy(texte_minute_bk,texte_minute);
+ difference=1;
+ }
+ if (strcmp(texte_heure, texte_heure_bk)!=0) {
+ efface(267,107,304,135);
+ X_print=275;
+ Y_print=110;
+ print_lettre(texte_heure[0]-32,taille_glob);
+ X_print=290;
+ print_lettre(texte_heure[1]-32,taille_glob);
+ strcpy(texte_heure_bk,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);
+ }
+
+ 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;
+}
+
+void efface_ligne (int line_nb)
+{
+ float i;
+ //set_outil(1);
+ switch (line_nb) {
+ case 1 :
+ set_outil(1,0,112);
+ //goto_2D_line(0,104);
+ //wait(2);
+ set_poignet(5);
+ for (i=0; i<31; i=i+10.6) {
+ goto_2D_line(0,112+i);//104
+ //print_heure();
+ goto_2D_line(120,116+i);//113
+ print_heure();
+ goto_2D_line(240,109+i);//104
+ print_heure();
+ //goto_2D_line(360,107+i);
+ //goto_2D_line(360,107+i+5);
+ goto_2D_line(265,108+i);
+ print_heure();
+ goto_2D_line(265,108+i+5.3);
+ print_heure();
+ goto_2D_line(240,109+i+5.3);
+ print_heure();
+ goto_2D_line(120,116+i+5.3);
+ print_heure();
+ goto_2D_line(0,112+i+5.3);
+ //print_heure();
+ }
+ break;
+ case 2 :
+ set_outil(1,0,70);
+ //goto_2D_line(0,70);
+ //wait(2);
+ set_poignet(5);
+ for (i=0; i<31; i=i+10.6) { //8
+ goto_2D_line(0,76+i);//104
+ //print_heure();
+ goto_2D_line(120,81+i);//113
+ print_heure();
+ goto_2D_line(240,78+i);//104
+ print_heure();
+ goto_2D_line(360,72+i);
+ print_heure();
+ goto_2D_line(360,72+i+5.3);
+ print_heure();
+ goto_2D_line(240,78+i+5.3);
+ print_heure();
+ goto_2D_line(120,81+i+5.3);
+ print_heure();
+ goto_2D_line(0,76+i+5.3);//4
+ //print_heure();
+ }
+ break;
+ case 3 :
+ //goto_2D_line(0,37);
+ set_outil(1,0,37);
+ //wait(2);
+ set_poignet(5);
+ cons_servo_main=745;
+ for (i=0; i<31; i=i+10.6) {
+ goto_2D_line(0,39+i);//104
+ //print_heure();
+ goto_2D_line(120,46+i);//113
+ print_heure();
+ goto_2D_line(240,47+i);//104
+ print_heure();
+ goto_2D_line(360,44+i);
+ print_heure();
+ goto_2D_line(360,44+i+5.3);
+ print_heure();
+ goto_2D_line(240,47+i+5.3);
+ print_heure();
+ goto_2D_line(120,46+i+5.3);
+ print_heure();
+ 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;
+ for (i=0; i<31; i=i+10.6) {
+ goto_2D_line(0,5+i);//104
+ //print_heure();
+ goto_2D_line(120,12+i);//113
+ print_heure();
+ goto_2D_line(240,12+i);//104
+ print_heure();
+ goto_2D_line(360,8+i);
+ print_heure();
+ goto_2D_line(360,8+i+5.3);
+ print_heure();
+ goto_2D_line(240,12+i+5.3);
+ print_heure();
+ goto_2D_line(120,12+i+5.3);
+ print_heure();
+ goto_2D_line(0,5+i+5.3);
+ }
+ break;
+ }
+ set_poignet(4);
+
+ //set_outil(0);
+}
+
+void retour_ligne(void)
+{
+ switch (ligne) {
+ case 1:
+ remplissage[1]=1;
+ if (remplissage[2]==1) {
+ efface_ligne(2);
+ }
+ X_print=1;
+ Y_print=77;
+ ligne=2;
+ break;
+ case 2:
+ remplissage[2]=1;
+ if (remplissage[3]==1) {
+ efface_ligne(3);
+ }
+ X_print=1;
+ Y_print=44;
+ ligne=3;
+ break;
+ case 3:
+ remplissage[3]=1;
+ if (remplissage[4]==1) {
+ efface_ligne(4);
+ }
+ X_print=1;
+ Y_print=11;
+ ligne=4;
+ break;
+ case 4:
+ remplissage[4]=1;
+ if (remplissage[1]==1) {
+ efface_ligne(1);
+ }
+ X_print=1;
+ Y_print=110;
+ ligne=1;
+ break;
+ }
+}
+
+void print_heure_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);
+ X_print=275;
+ Y_print=110;
+ print_lettre(texte_heure_bk[0]-32,taille_glob);
+ X_print=290;
+ print_lettre(texte_heure_bk[1]-32,taille_glob);
+ X_print=310;
+ print_lettre(104-32,taille_glob);
+ X_print=330;
+ print_lettre(texte_minute_bk[0]-32,taille_glob);
+ X_print=345;
+ print_lettre(texte_minute_bk[1]-32,taille_glob);
+}
+
+
+void print_chaine(char* chaine)
+{
+ int i=0;
+ int i_fictif=0;
+ int retour_ligne_loc=0;
+ int rupt=0;
+ while (chaine[i]!=0) {
+ print_heure();
+ retour_ligne_loc=0;
+ //printf("interp: %d\r\n",chaine[i]);
+ if (chaine[i]>31) {
+ print_lettre(chaine[i]-32,taille_glob);
+ } else if (chaine[i]==10) {
+ retour_ligne();
+ //X_print=1;
+ //Y_print=Y_print-22*taille_glob;
+ }
+ 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;
+ }
+ //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;
+ 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);
+ }
+ //pc.printf("Xfictif %f\r\n",X_fictif);
+ if ((X_fictif>345)||((ligne==1)&&(X_fictif>270))) {
+ rupt=2;
+ }
+ if ((chaine[i_fictif]==32)||(chaine[i_fictif]==10)||(chaine[i_fictif]==0)) {
+ rupt=1;
+ //pc.printf("rupt 1 %d\r\n",chaine[i_fictif]);
+ }
+ i_fictif++;
+ }
+ if (rupt==2) {
+ retour_ligne();
+ //X_print=1;
+ //Y_print=Y_print-22*taille_glob;
+ //pc.printf("rupt 2\r\n");
+ }
+ }
+ i++;
+ }
+}
+
+
+
+
+
+int main()
+{
+ FILE* fileid;
+ int i,j,k;
+ char montexte[600];
+ float aaa, bbb, ccc, ddd, eee;
+
+ struct tm * timeinfo;
+
+ time_t seconds = time(NULL);
+ // ********* initialisations *******
+ //while(1){}
+ pc.baud(115200);
+ pc.printf("bijour\r\n");
+ tick_attached_coude=0;
+ tick_attached_epaule=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);
+ //}
+
+ //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;
+ 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);
+
+ montexte[500]=0;
+ fclose(fileid);
+ for (i=0; i<38; i++) {
+ printf(":%d\r\n",montexte[i]);
+ }
+ pc.printf("Time brut = %d\r\n", seconds);
+ pc.printf("Time = %s", ctime(&seconds));
+ //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();
+ 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);
+
+ 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;
+ 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);
+ }
+}