File content as of revision 1:163fa4a925d9:

#include "mbed.h"
#include "time.h"

#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);
DigitalOut stepper_dir_1(p22);
DigitalOut stepper_clk_2(p23);
DigitalOut stepper_dir_2(p24);
DigitalOut servoout(p17);
AnalogIn potard_1 (p20);
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 servo_call;

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_fictive;
int line_filling_st[5]= {0,0,0,0,0};
char hour_txt_back[10];
char minute_txt_back[10];

float glob_size;

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 ' 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 , 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 /
    ,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, wrist_servo_setpoint);
}

void clk_stepper1(void)
{
    stepper_clk_1=1;
    pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
    a_elbow=a_elbow + dir_1_real;
    stepper_clk_1=0;
}

void clk_stepper2(void)
{
    stepper_clk_2=1;
    pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
    a_shoulder=a_shoulder + dir_2_real;
    stepper_clk_2=0;
}

void tick_detach()
{
    tick_attached_elbow=0;
    tick_attached_shoulder=0;
    pulse_stepper1.detach();
    pulse_stepper2.detach();
}

void set_wrist(int wrist_pos)
{
    switch (wrist_pos) {
        case 1 : //laid pen
            if (wrist_servo_setpoint!=laid_pen_servopos) {
                wait_ms(100);
                wrist_servo_setpoint=laid_pen_servopos;
                myled=1;
                wait_ms(80);
            }
            accurate=1;
            break;
        case 2 : //raised pen
            if (wrist_servo_setpoint!=raised_pen_servopos) {
                //wait_ms(50);
                wrist_servo_setpoint=raised_pen_servopos;
                myled=0;
                wait_ms(50);
            }
            accurate=0;
            break;
        case 3 : //neutral
            wrist_servo_setpoint=neutral_pen_servopos;
            accurate=0;
            break;
        case 4 : //eraser raised
            wrist_servo_setpoint=raised_eraser_servopos;
            accurate=0;
            break;
        case 5 : //eraser laid
            wrist_servo_setpoint=laid_eraser_servopos;
            accurate=2;
    }
}



void init_stepper()
{
    //int arret=0;
    int i;
    while (switch_shoulder==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_elbow==1) {
        stepper_clk_1=1;
        wait_us(10);
        stepper_clk_1=0;
        wait_ms(3);
    }
    a_elbow=-2940;//2900
    a_shoulder=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);
    //}
    //wrist_servo_setpoint=1800;
}

void setpos_ang (int a_elbow_target, int a_shoulder_target, int delay_ms)
{
    int err_a_elbow, err_a_shoulder;
    float delay_1, delay_2;
    //pulse_stepper1.detach();
    //pulse_stepper2.detach();
    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_shoulder>0) {
        stepper_dir_2=1;
        dir_2_real=1;
    } else {
        stepper_dir_2=0;
        dir_2_real=-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_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;
            led_err=0;
        }
        period_stepper1=delay_1-3;
        if (tick_attached_elbow==0) {
            pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
            tick_attached_elbow=1;
        }
    } else {
        tick_attached_elbow=0;
        pulse_stepper1.detach();
    }

    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_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;
            led_err=0;
        }
        period_stepper2=delay_2-3;
        if (tick_attached_shoulder==0) {
            pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
            tick_attached_shoulder=1;
        }
    } else {
        tick_attached_shoulder=0;
        pulse_stepper2.detach();
    }
    wait_ms(delay_ms);
}

void goto_2D_short (float X_target, float Y_target, int delay_ms)
{
    float arm_extension, deltax, deltay, alpha_D, arad_elbow_target, arad_shoulder_target, setpoint_elbow, setpoint_shoulder, error_elbow, error_shoulder;
    int delay_loc;
    // simplified inverse kinematics
    if (X_target > 360) {
        X_target=360;
    }
    if (X_target < 0) {
        X_target=0;
    }
    if (Y_target > 145) {
        Y_target=145;   //110
    }
    if (Y_target < 0) {
        Y_target=0;
    }
    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 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_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;
    }

    if (delay_ms==0) {
        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*error_shoulder;
        }
        setpos_ang (setpoint_elbow, setpoint_shoulder, delay_loc);
        X_pos=X_target;
        Y_pos=Y_target;
    } else  {
        setpos_ang (setpoint_elbow, setpoint_shoulder, delay_ms);
        X_pos=X_target;
        Y_pos=Y_target;
    }
}

void goto_2D_line (float X_target, float Y_target)
{
    float delta_x, delta_y, distance, x_decomp, y_decomp;
    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));
    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;
    for (i=1; i<=step_nb; i++) {
        x_decomp=x_decomp+delta_x;
        y_decomp=y_decomp+delta_y;
        if(accurate==1) {
            goto_2D_short (x_decomp, y_decomp, 10+70*potard_1.read());   //12
        } else if (accurate==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 radius, float angle_deb, float angle_fin)
{
    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));
    if (distance>2) {
        set_wrist(2);
        goto_2D_line (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_tool(int tool_loc, float X_loc, float Y_loc)
{
    if ((tool_loc==1)&&(tool==0)) {//eraser
        set_wrist(3);
        tool=1;
        goto_2D_short (X_loc, Y_loc,0);//2000
        tick_detach();
        set_wrist(4);
        wait_ms(200);
    } else if ((tool_loc==0)&&(tool==1)) {
        set_wrist(3);
        tool=0;
        goto_2D_short (X_loc, Y_loc,0);//2000
        tick_detach();
        set_wrist(2);
        wait_ms(200);
    } else if ((tool_loc==1)&&(tool==1)) {
        set_wrist(4);
    }
}

void scr_print_single_char(char charact, float size)
{
    int s_end_char=0;
    int i_instr=0;
    int x_temp, y_temp, charact_loc;
    float X_local, Y_local;
    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", 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+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+size*y_temp;
                goto_2D_line(X_local, Y_local);
                i_instr=i_instr+3;
                break;
            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+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+size*y_temp;
                goto_2D_line(X_local, Y_local);
                i_instr=i_instr+3;
                break;
            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+size*x_temp;
                y_temp=font[charact_loc][i_instr+2];
                if (y_temp>127) {
                    y_temp=y_temp-256;
                }
                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, radius_loc, angle_deb_loc, angle_fin_loc);
                i_instr=i_instr+6;
                break;
            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+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_wrist(1);
                goto_2D_line(X_local, Y_local);
                set_wrist(2);
                i_instr=i_instr+3;
                break;
        }
    }
}

void scr_print_single_char_fictive(char charact, float size)
{
    int s_end_char=0;
    int i_instr=0;
    int x_temp, y_temp, charact_loc;
    float X_local, Y_local;
    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 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 : // line pen pose
                i_instr=i_instr+3;
                break;
            case 4 : // circle
                i_instr=i_instr+6;
                break;
            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;
                break;
        }
    }
}

void erase(float Xl1, float Yl1, float Xl2, float Yl2)
{
    float distance_Y,pas, Y_loc;
    int i, step_nb, tool_old_loc;
    tool_old_loc=tool;
    set_tool(1, Xl1, Yl1);
    goto_2D_line(Xl1,Yl1);
    set_wrist(5);
    distance_Y=Yl2-Yl1;
    Y_loc=Yl1;

    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_wrist(4);
}


void scrib_print_hour(void)
{
    char texte_heure[10];
    char texte_minute[10];
    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);
    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;
    tool_bk=tool;
    accurate_bk=accurate;
    difference=0;
    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;
        scr_print_single_char(texte_minute[0]-32,glob_size);
        X_print=345;
        scr_print_single_char(texte_minute[1]-32,glob_size);
        strcpy(minute_txt_back,texte_minute);
        difference=1;
    }
    if (strcmp(texte_heure, hour_txt_back)!=0) {
        erase(267,107,304,135);
        X_print=275;
        Y_print=110;
        scr_print_single_char(texte_heure[0]-32,glob_size);
        X_print=290;
        scr_print_single_char(texte_heure[1]-32,glob_size);
        strcpy(hour_txt_back,texte_heure);
        difference=1;
    }
    if ((tool_bk==1)&&(difference==1)) {
        set_tool(1,X_pos_bk,Y_pos_bk);
    }

    X_print=X_backup;
    Y_print=Y_backup;

    wrist_servo_setpoint=wrist_servo_setpoint_bk;
    accurate=accurate_bk;
}

void erase_line (int line_nb)
{
    float i;
    //set_tool(1);
    switch (line_nb) {
        case 1 :
            set_tool(1,0,112);

            set_wrist(5);
            for (i=0; i<31; i=i+10.6) {
                goto_2D_line(0,112+i);//104
                //scrib_print_hour();
                goto_2D_line(120,116+i);//113
                scrib_print_hour();
                goto_2D_line(240,109+i);//104
                scrib_print_hour();

                goto_2D_line(265,108+i);
                scrib_print_hour();
                goto_2D_line(265,108+i+5.3);
                scrib_print_hour();
                goto_2D_line(240,109+i+5.3);
                scrib_print_hour();
                goto_2D_line(120,116+i+5.3);
                scrib_print_hour();
                goto_2D_line(0,112+i+5.3);
                //scrib_print_hour();
            }
            break;
        case 2 :
            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
                //scrib_print_hour();
                goto_2D_line(120,81+i);//113
                scrib_print_hour();
                goto_2D_line(240,78+i);//104
                scrib_print_hour();
                goto_2D_line(360,72+i);
                scrib_print_hour();
                goto_2D_line(360,72+i+5.3);
                scrib_print_hour();
                goto_2D_line(240,78+i+5.3);
                scrib_print_hour();
                goto_2D_line(120,81+i+5.3);
                scrib_print_hour();
                goto_2D_line(0,76+i+5.3);//4
                //scrib_print_hour();
            }
            break;
        case 3 :

            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
                //scrib_print_hour();
                goto_2D_line(120,46+i);//113
                scrib_print_hour();
                goto_2D_line(240,47+i);//104
                scrib_print_hour();
                goto_2D_line(360,44+i);
                scrib_print_hour();
                goto_2D_line(360,44+i+5.3);
                scrib_print_hour();
                goto_2D_line(240,47+i+5.3);
                scrib_print_hour();
                goto_2D_line(120,46+i+5.3);
                scrib_print_hour();
                goto_2D_line(0,39+i+5.3);
            }
            break;

        case 4 :

            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
                //scrib_print_hour();
                goto_2D_line(120,12+i);//113
                scrib_print_hour();
                goto_2D_line(240,12+i);//104
                scrib_print_hour();
                goto_2D_line(360,8+i);
                scrib_print_hour();
                goto_2D_line(360,8+i+5.3);
                scrib_print_hour();
                goto_2D_line(240,12+i+5.3);
                scrib_print_hour();
                goto_2D_line(120,12+i+5.3);
                scrib_print_hour();
                goto_2D_line(0,5+i+5.3);
            }
            break;
    }
    set_wrist(4);

}

void scrib_line_return(void)
{
    switch (line) {
        case 1:
            line_filling_st[1]=1;
            if (line_filling_st[2]==1) {
                erase_line(2);
            }
            X_print=1;
            Y_print=77;
            line=2;
            break;
        case 2:
            line_filling_st[2]=1;
            if (line_filling_st[3]==1) {
                erase_line(3);
            }
            X_print=1;
            Y_print=44;
            line=3;
            break;
        case 3:
            line_filling_st[3]=1;
            if (line_filling_st[4]==1) {
                erase_line(4);
            }
            X_print=1;
            Y_print=11;
            line=4;
            break;
        case 4:
            line_filling_st[4]=1;
            if (line_filling_st[1]==1) {
                erase_line(1);
            }
            X_print=1;
            Y_print=110;
            line=1;
            break;
    }
}

void scrib_print_hour_initial(void)
{
    char texte_heure[10];

    struct tm * timeinfo;
    time_t seconds = time(NULL);
    timeinfo = localtime (&seconds);
    strftime(hour_txt_back,10,"%H",timeinfo);
    strftime(minute_txt_back,10,"%M",timeinfo);
    X_print=275;
    Y_print=110;
    scr_print_single_char(hour_txt_back[0]-32,glob_size);
    X_print=290;
    scr_print_single_char(hour_txt_back[1]-32,glob_size);
    X_print=310;
    scr_print_single_char(104-32,glob_size);
    X_print=330;
    scr_print_single_char(minute_txt_back[0]-32,glob_size);
    X_print=345;
    scr_print_single_char(minute_txt_back[1]-32,glob_size);
}


void scrib_print_string(char* chaine)
{
    int i=0;
    int i_fictive=0;
    int line_feed_loc=0;
    int rupt=0;
    while (chaine[i]!=0) {
        scrib_print_hour();
        line_feed_loc=0;
        if (chaine[i]>31) {
            scr_print_single_char(chaine[i]-32,glob_size);
        } else if (chaine[i]==10) {
            scrib_line_return();

        }
        if ((X_print>345)||((line==1)&&(X_print>270))) {
            scrib_line_return();

            line_feed_loc=1;
        }
        if ((chaine[i]==32)&&(line_feed_loc==0)) { //determine 
            X_fictive=X_print;
            i_fictive=i+1;
            rupt=0;
            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);
                }
                if ((X_fictive>345)||((line==1)&&(X_fictive>270))) {
                    rupt=2;
                }
                if ((chaine[i_fictive]==32)||(chaine[i_fictive]==10)||(chaine[i_fictive]==0)) {
                    rupt=1;
                }
                i_fictive++;
            }
            if (rupt==2) {
                scrib_line_return();

            }
        }
        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);
    // ********* initializations *******

    pc.baud(115200);
    pc.printf("bijour\r\n");
    tick_attached_elbow=0;
    tick_attached_shoulder=0;
    X_print=1;
    Y_print=110; //110
    line=1;
    accurate=0;
    tool=0;
    glob_size=1.5;
    servo_call.attach_us(&tick_servo, 20000);

    wrist_servo_setpoint=neutral_pen_servopos;
    wait(1);
    

    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);


    init_stepper();



    myled=!myled;

    goto_2D_short (0,110, 1000);
    tick_detach();


    scrib_print_hour_initial();
    X_print=1;
    Y_print=110;//110

    while(1) {
        scrib_print_string(montexte);

    }
    while(1) {}

    set_wrist(3);

    while(1) {}

    a_elbow=0;
    a_shoulder=0;
    aaa=1000;
    bbb=0;

}