Guillaume F4HDK / Mbed 2 deprecated scribe_stepper_2

scribing robot see https://hackaday.io/project/153194/

Dependencies:   mbed

See description of the project here: https://hackaday.io/project/153194/

main.cpp@1:163fa4a925d9, 2018-04-22 (annotated)

Committer:
Leon
Date:
Sun Apr 22 14:00:17 2018 +0000
Revision:
1:163fa4a925d9
Parent:
0:694620b4c620 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Leon 0:694620b4c620 1 #include "mbed.h"
Leon 0:694620b4c620 2 #include "time.h"
Leon 0:694620b4c620 3
Leon 1:163fa4a925d9 4 #define laid_pen_servopos 1800//1800
Leon 1:163fa4a925d9 5 #define raised_pen_servopos 1680//1680
Leon 1:163fa4a925d9 6 #define neutral_pen_servopos 1400
Leon 1:163fa4a925d9 7 #define raised_eraser_servopos 900//900
Leon 1:163fa4a925d9 8 #define laid_eraser_servopos 755//730
Leon 0:694620b4c620 9
Leon 0:694620b4c620 10 DigitalOut myled(LED1);
Leon 0:694620b4c620 11 DigitalOut led_err(LED3);
Leon 1:163fa4a925d9 12 DigitalOut stepper_clk_1(p21);
Leon 1:163fa4a925d9 13 DigitalOut stepper_dir_1(p22);
Leon 1:163fa4a925d9 14 DigitalOut stepper_clk_2(p23);
Leon 1:163fa4a925d9 15 DigitalOut stepper_dir_2(p24);
Leon 0:694620b4c620 16 DigitalOut servoout(p17);
Leon 0:694620b4c620 17 AnalogIn potard_1 (p20);
Leon 1:163fa4a925d9 18 DigitalIn switch_shoulder(p18);
Leon 1:163fa4a925d9 19 DigitalIn switch_elbow(p19);
Leon 0:694620b4c620 20 Serial pc(USBTX, USBRX);
Leon 0:694620b4c620 21 LocalFileSystem local("local");
Leon 0:694620b4c620 22
Leon 0:694620b4c620 23 Timeout pulse_stepper1;
Leon 0:694620b4c620 24 Timeout pulse_stepper2;
Leon 0:694620b4c620 25 Timeout servo_timeout;
Leon 1:163fa4a925d9 26 Ticker servo_call;
Leon 0:694620b4c620 27
Leon 1:163fa4a925d9 28 int wrist_servo_setpoint=1400; //1800
Leon 1:163fa4a925d9 29 int a_elbow, a_shoulder, dir_1_real, dir_2_real, tick_attached_elbow, tick_attached_shoulder;
Leon 1:163fa4a925d9 30 int period_stepper1, period_stepper2, accurate, tool, line;
Leon 0:694620b4c620 31 float X_pos, Y_pos;
Leon 1:163fa4a925d9 32 float X_print, Y_print, X_fictive;
Leon 1:163fa4a925d9 33 int line_filling_st[5]= {0,0,0,0,0};
Leon 1:163fa4a925d9 34 char hour_txt_back[10];
Leon 1:163fa4a925d9 35 char minute_txt_back[10];
Leon 0:694620b4c620 36
Leon 1:163fa4a925d9 37 float glob_size;
Leon 0:694620b4c620 38
Leon 1:163fa4a925d9 39 char font[256][30]= {
Leon 1:163fa4a925d9 40 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
Leon 0:694620b4c620 41 ,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 !
Leon 0:694620b4c620 42 ,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 "
Leon 0:694620b4c620 43 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 44 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 45 ,5,0,0,0,0,0,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 %
Leon 0:694620b4c620 46 ,5,0,0,0,0,0,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 &
Leon 1:163fa4a925d9 47 ,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
Leon 0:694620b4c620 48 ,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 (
Leon 0:694620b4c620 49 ,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 )
Leon 0:694620b4c620 50 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 51 ,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 +
Leon 1:163fa4a925d9 52 ,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
Leon 0:694620b4c620 53 ,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 -
Leon 0:694620b4c620 54 ,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
Leon 0:694620b4c620 55 ,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 /
Leon 0:694620b4c620 56 ,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
Leon 0:694620b4c620 57 ,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
Leon 0:694620b4c620 58 ,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
Leon 0:694620b4c620 59 ,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
Leon 0:694620b4c620 60 ,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
Leon 0:694620b4c620 61 ,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
Leon 0:694620b4c620 62 ,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
Leon 0:694620b4c620 63 ,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
Leon 0:694620b4c620 64 ,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
Leon 0:694620b4c620 65 ,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
Leon 0:694620b4c620 66 ,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 :
Leon 0:694620b4c620 67 ,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 ;
Leon 0:694620b4c620 68 ,5,0,0,0,0,0,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 <
Leon 0:694620b4c620 69 ,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 =
Leon 0:694620b4c620 70 ,5,0,0,0,0,0,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 >
Leon 0:694620b4c620 71 ,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 ?
Leon 0:694620b4c620 72 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 73
Leon 0:694620b4c620 74 ,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
Leon 0:694620b4c620 75 ,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
Leon 0:694620b4c620 76 ,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
Leon 0:694620b4c620 77 ,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
Leon 0:694620b4c620 78 ,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
Leon 0:694620b4c620 79 ,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
Leon 0:694620b4c620 80 ,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
Leon 0:694620b4c620 81 ,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
Leon 0:694620b4c620 82 ,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
Leon 0:694620b4c620 83 ,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
Leon 0:694620b4c620 84 ,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
Leon 0:694620b4c620 85 ,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
Leon 0:694620b4c620 86 ,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
Leon 0:694620b4c620 87 ,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
Leon 0:694620b4c620 88 ,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
Leon 0:694620b4c620 89 ,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
Leon 0:694620b4c620 90 ,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
Leon 0:694620b4c620 91 ,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
Leon 0:694620b4c620 92 ,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
Leon 0:694620b4c620 93 ,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
Leon 0:694620b4c620 94 ,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
Leon 0:694620b4c620 95 ,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
Leon 0:694620b4c620 96 ,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
Leon 0:694620b4c620 97 ,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
Leon 0:694620b4c620 98 ,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
Leon 0:694620b4c620 99 ,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
Leon 0:694620b4c620 100
Leon 0:694620b4c620 101 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 102 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 103 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 104 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 105 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 106 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 107
Leon 0:694620b4c620 108 ,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
Leon 0:694620b4c620 109 ,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
Leon 0:694620b4c620 110 ,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
Leon 0:694620b4c620 111 ,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
Leon 0:694620b4c620 112 ,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
Leon 0:694620b4c620 113 ,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
Leon 0:694620b4c620 114 ,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
Leon 0:694620b4c620 115 ,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
Leon 0:694620b4c620 116 ,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
Leon 0:694620b4c620 117 ,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
Leon 0:694620b4c620 118 ,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
Leon 0:694620b4c620 119 ,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
Leon 0:694620b4c620 120 ,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
Leon 0:694620b4c620 121 ,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
Leon 0:694620b4c620 122 ,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
Leon 0:694620b4c620 123 ,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
Leon 0:694620b4c620 124 ,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
Leon 0:694620b4c620 125 ,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
Leon 0:694620b4c620 126 ,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
Leon 0:694620b4c620 127 ,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
Leon 0:694620b4c620 128 ,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
Leon 0:694620b4c620 129 ,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
Leon 0:694620b4c620 130 ,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
Leon 0:694620b4c620 131 ,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
Leon 0:694620b4c620 132 ,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
Leon 0:694620b4c620 133 ,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
Leon 0:694620b4c620 134
Leon 0:694620b4c620 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 //123
Leon 0:694620b4c620 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 //124
Leon 0:694620b4c620 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 //125
Leon 0:694620b4c620 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 //126
Leon 0:694620b4c620 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 //127
Leon 0:694620b4c620 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 //128
Leon 0:694620b4c620 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 //129
Leon 0:694620b4c620 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 //130
Leon 0:694620b4c620 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 //131
Leon 0:694620b4c620 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 //132
Leon 0:694620b4c620 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 //133
Leon 0:694620b4c620 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 //134
Leon 0:694620b4c620 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 //135
Leon 0:694620b4c620 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 //136
Leon 0:694620b4c620 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 //137
Leon 0:694620b4c620 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 //138
Leon 0:694620b4c620 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 //139
Leon 0:694620b4c620 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 //140
Leon 0:694620b4c620 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 //141
Leon 0:694620b4c620 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 //142
Leon 0:694620b4c620 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 //143
Leon 0:694620b4c620 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 //144
Leon 0:694620b4c620 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 //145
Leon 0:694620b4c620 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 //146
Leon 0:694620b4c620 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 //147
Leon 0:694620b4c620 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 //148
Leon 0:694620b4c620 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 //149
Leon 0:694620b4c620 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 //150
Leon 0:694620b4c620 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 //151
Leon 0:694620b4c620 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 //152
Leon 0:694620b4c620 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 //153
Leon 0:694620b4c620 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 //154
Leon 0:694620b4c620 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 //155
Leon 0:694620b4c620 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 //156
Leon 0:694620b4c620 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 //157
Leon 0:694620b4c620 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 //158
Leon 0:694620b4c620 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 //159
Leon 0:694620b4c620 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 //160
Leon 0:694620b4c620 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 //161
Leon 0:694620b4c620 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 //162
Leon 0:694620b4c620 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 //163
Leon 0:694620b4c620 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 //164
Leon 0:694620b4c620 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 //165
Leon 0:694620b4c620 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 //166
Leon 0:694620b4c620 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 //167
Leon 0:694620b4c620 180 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 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 //169
Leon 0:694620b4c620 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 //170
Leon 0:694620b4c620 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 //171
Leon 0:694620b4c620 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 //172
Leon 0:694620b4c620 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 //173
Leon 0:694620b4c620 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 //174
Leon 0:694620b4c620 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 //175
Leon 0:694620b4c620 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 //176
Leon 0:694620b4c620 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 //177 +/-
Leon 0:694620b4c620 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 //178
Leon 0:694620b4c620 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 //179
Leon 0:694620b4c620 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 //180
Leon 0:694620b4c620 193 ,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 µ
Leon 0:694620b4c620 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 //182
Leon 0:694620b4c620 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 //183
Leon 0:694620b4c620 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 //184
Leon 0:694620b4c620 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 //185
Leon 0:694620b4c620 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 //186
Leon 0:694620b4c620 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 //187
Leon 0:694620b4c620 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 //188
Leon 0:694620b4c620 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 //189
Leon 0:694620b4c620 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 //190
Leon 0:694620b4c620 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 //191
Leon 0:694620b4c620 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 //192
Leon 0:694620b4c620 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 //193
Leon 0:694620b4c620 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 //194
Leon 0:694620b4c620 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 //195
Leon 0:694620b4c620 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 //196
Leon 0:694620b4c620 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 //197
Leon 0:694620b4c620 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 //198
Leon 0:694620b4c620 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 //199
Leon 0:694620b4c620 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 //200
Leon 0:694620b4c620 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 //201
Leon 0:694620b4c620 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 //202
Leon 0:694620b4c620 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 //203
Leon 0:694620b4c620 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 //204
Leon 0:694620b4c620 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 //205
Leon 0:694620b4c620 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 //206
Leon 0:694620b4c620 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 //207
Leon 0:694620b4c620 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 //208
Leon 0:694620b4c620 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 //209
Leon 0:694620b4c620 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 //210
Leon 0:694620b4c620 223 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 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 //212
Leon 0:694620b4c620 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 //213
Leon 0:694620b4c620 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 //214
Leon 0:694620b4c620 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 //215
Leon 0:694620b4c620 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 //216
Leon 0:694620b4c620 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 //217
Leon 0:694620b4c620 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 //218
Leon 0:694620b4c620 231 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 232 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 233 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 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 //222
Leon 0:694620b4c620 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 //223
Leon 0:694620b4c620 236 ,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 à
Leon 0:694620b4c620 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 //225
Leon 0:694620b4c620 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 //226
Leon 0:694620b4c620 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 //227
Leon 0:694620b4c620 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 //228
Leon 0:694620b4c620 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 //229
Leon 0:694620b4c620 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 //230
Leon 0:694620b4c620 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 //231
Leon 0:694620b4c620 244 ,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 è
Leon 0:694620b4c620 245 ,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 é
Leon 0:694620b4c620 246 ,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 ê
Leon 0:694620b4c620 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 //235
Leon 0:694620b4c620 248 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 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 //237
Leon 0:694620b4c620 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 //238
Leon 0:694620b4c620 251 ,5,0,0,0,0,0,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 ï
Leon 0:694620b4c620 252 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 253 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 254 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 255 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 256 ,5,0,0,0,0,0,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 ô
Leon 0:694620b4c620 257 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 258 ,5,0,0,0,0,0,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 ö
Leon 0:694620b4c620 259 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 260 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 261 ,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 ù
Leon 0:694620b4c620 262 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 263 ,5,0,0,0,0,0,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
Leon 0:694620b4c620 264 };
Leon 0:694620b4c620 265
Leon 0:694620b4c620 266 void stop_tick_servo(void)
Leon 0:694620b4c620 267 {
Leon 0:694620b4c620 268 servoout=0;
Leon 0:694620b4c620 269 }
Leon 0:694620b4c620 270
Leon 0:694620b4c620 271 void tick_servo (void)
Leon 0:694620b4c620 272 {
Leon 0:694620b4c620 273 servoout=1;
Leon 1:163fa4a925d9 274 servo_timeout.attach_us(&stop_tick_servo, wrist_servo_setpoint);
Leon 0:694620b4c620 275 }
Leon 0:694620b4c620 276
Leon 0:694620b4c620 277 void clk_stepper1(void)
Leon 0:694620b4c620 278 {
Leon 0:694620b4c620 279 stepper_clk_1=1;
Leon 0:694620b4c620 280 pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
Leon 1:163fa4a925d9 281 a_elbow=a_elbow + dir_1_real;
Leon 0:694620b4c620 282 stepper_clk_1=0;
Leon 0:694620b4c620 283 }
Leon 0:694620b4c620 284
Leon 0:694620b4c620 285 void clk_stepper2(void)
Leon 0:694620b4c620 286 {
Leon 0:694620b4c620 287 stepper_clk_2=1;
Leon 0:694620b4c620 288 pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
Leon 1:163fa4a925d9 289 a_shoulder=a_shoulder + dir_2_real;
Leon 0:694620b4c620 290 stepper_clk_2=0;
Leon 0:694620b4c620 291 }
Leon 0:694620b4c620 292
Leon 0:694620b4c620 293 void tick_detach()
Leon 0:694620b4c620 294 {
Leon 1:163fa4a925d9 295 tick_attached_elbow=0;
Leon 1:163fa4a925d9 296 tick_attached_shoulder=0;
Leon 0:694620b4c620 297 pulse_stepper1.detach();
Leon 0:694620b4c620 298 pulse_stepper2.detach();
Leon 0:694620b4c620 299 }
Leon 0:694620b4c620 300
Leon 1:163fa4a925d9 301 void set_wrist(int wrist_pos)
Leon 0:694620b4c620 302 {
Leon 1:163fa4a925d9 303 switch (wrist_pos) {
Leon 1:163fa4a925d9 304 case 1 : //laid pen
Leon 1:163fa4a925d9 305 if (wrist_servo_setpoint!=laid_pen_servopos) {
Leon 0:694620b4c620 306 wait_ms(100);
Leon 1:163fa4a925d9 307 wrist_servo_setpoint=laid_pen_servopos;
Leon 0:694620b4c620 308 myled=1;
Leon 0:694620b4c620 309 wait_ms(80);
Leon 0:694620b4c620 310 }
Leon 1:163fa4a925d9 311 accurate=1;
Leon 0:694620b4c620 312 break;
Leon 1:163fa4a925d9 313 case 2 : //raised pen
Leon 1:163fa4a925d9 314 if (wrist_servo_setpoint!=raised_pen_servopos) {
Leon 0:694620b4c620 315 //wait_ms(50);
Leon 1:163fa4a925d9 316 wrist_servo_setpoint=raised_pen_servopos;
Leon 0:694620b4c620 317 myled=0;
Leon 0:694620b4c620 318 wait_ms(50);
Leon 0:694620b4c620 319 }
Leon 1:163fa4a925d9 320 accurate=0;
Leon 0:694620b4c620 321 break;
Leon 1:163fa4a925d9 322 case 3 : //neutral
Leon 1:163fa4a925d9 323 wrist_servo_setpoint=neutral_pen_servopos;
Leon 1:163fa4a925d9 324 accurate=0;
Leon 0:694620b4c620 325 break;
Leon 1:163fa4a925d9 326 case 4 : //eraser raised
Leon 1:163fa4a925d9 327 wrist_servo_setpoint=raised_eraser_servopos;
Leon 1:163fa4a925d9 328 accurate=0;
Leon 0:694620b4c620 329 break;
Leon 1:163fa4a925d9 330 case 5 : //eraser laid
Leon 1:163fa4a925d9 331 wrist_servo_setpoint=laid_eraser_servopos;
Leon 1:163fa4a925d9 332 accurate=2;
Leon 0:694620b4c620 333 }
Leon 0:694620b4c620 334 }
Leon 0:694620b4c620 335
Leon 0:694620b4c620 336
Leon 0:694620b4c620 337
Leon 0:694620b4c620 338 void init_stepper()
Leon 0:694620b4c620 339 {
Leon 0:694620b4c620 340 //int arret=0;
Leon 0:694620b4c620 341 int i;
Leon 1:163fa4a925d9 342 while (switch_shoulder==1) {
Leon 0:694620b4c620 343 stepper_clk_2=1;
Leon 0:694620b4c620 344 wait_us(10);
Leon 0:694620b4c620 345 stepper_clk_2=0;
Leon 0:694620b4c620 346 wait_ms(3);
Leon 0:694620b4c620 347 }
Leon 0:694620b4c620 348 stepper_dir_2=1;
Leon 0:694620b4c620 349 for (i=1; i<2620; i++) { //2640
Leon 0:694620b4c620 350 stepper_clk_2=1;
Leon 0:694620b4c620 351 wait_us(10);
Leon 0:694620b4c620 352 stepper_clk_2=0;
Leon 0:694620b4c620 353 wait_ms(1);
Leon 0:694620b4c620 354 }
Leon 1:163fa4a925d9 355 while (switch_elbow==1) {
Leon 0:694620b4c620 356 stepper_clk_1=1;
Leon 0:694620b4c620 357 wait_us(10);
Leon 0:694620b4c620 358 stepper_clk_1=0;
Leon 0:694620b4c620 359 wait_ms(3);
Leon 0:694620b4c620 360 }
Leon 1:163fa4a925d9 361 a_elbow=-2940;//2900
Leon 1:163fa4a925d9 362 a_shoulder=0;
Leon 0:694620b4c620 363 //stepper_dir_1=1;
Leon 0:694620b4c620 364 // for (i=1;i<2900;i++) {
Leon 0:694620b4c620 365 // stepper_clk_1=1;
Leon 0:694620b4c620 366 // wait_us(10);
Leon 0:694620b4c620 367 // stepper_clk_1=0;
Leon 0:694620b4c620 368 // wait_ms(1);
Leon 0:694620b4c620 369 //}
Leon 1:163fa4a925d9 370 //wrist_servo_setpoint=1800;
Leon 0:694620b4c620 371 }
Leon 0:694620b4c620 372
Leon 1:163fa4a925d9 373 void setpos_ang (int a_elbow_target, int a_shoulder_target, int delay_ms)
Leon 0:694620b4c620 374 {
Leon 1:163fa4a925d9 375 int err_a_elbow, err_a_shoulder;
Leon 0:694620b4c620 376 float delay_1, delay_2;
Leon 0:694620b4c620 377 //pulse_stepper1.detach();
Leon 0:694620b4c620 378 //pulse_stepper2.detach();
Leon 1:163fa4a925d9 379 err_a_elbow=a_elbow_target - a_elbow;
Leon 1:163fa4a925d9 380 err_a_shoulder=a_shoulder_target - a_shoulder;
Leon 1:163fa4a925d9 381 if (err_a_elbow>0) {
Leon 0:694620b4c620 382 stepper_dir_1=1;
Leon 0:694620b4c620 383 dir_1_real=1;
Leon 0:694620b4c620 384 } else {
Leon 0:694620b4c620 385 stepper_dir_1=0;
Leon 0:694620b4c620 386 dir_1_real=-1;
Leon 0:694620b4c620 387 };
Leon 1:163fa4a925d9 388 if (err_a_shoulder>0) {
Leon 0:694620b4c620 389 stepper_dir_2=1;
Leon 0:694620b4c620 390 dir_2_real=1;
Leon 0:694620b4c620 391 } else {
Leon 0:694620b4c620 392 stepper_dir_2=0;
Leon 0:694620b4c620 393 dir_2_real=-1;
Leon 0:694620b4c620 394 };
Leon 1:163fa4a925d9 395 err_a_elbow=abs(err_a_elbow);
Leon 1:163fa4a925d9 396 err_a_shoulder=abs(err_a_shoulder);
Leon 1:163fa4a925d9 397 if (err_a_elbow !=0) {
Leon 1:163fa4a925d9 398 delay_1=delay_ms*1000/err_a_elbow;
Leon 0:694620b4c620 399 if (delay_1<250) {
Leon 0:694620b4c620 400 pc.printf("X:%f Y:%f\r\n",X_pos, Y_pos);
Leon 0:694620b4c620 401 printf("delay1 : %f\r\n",delay_1);
Leon 1:163fa4a925d9 402 printf("a_elbow:%d a_elbow_target: %d err_elbow : %d\r\n\r\n",a_elbow, a_elbow_target, err_a_elbow);
Leon 0:694620b4c620 403
Leon 0:694620b4c620 404 delay_1=250;
Leon 0:694620b4c620 405 led_err=1;
Leon 0:694620b4c620 406 led_err=0;
Leon 0:694620b4c620 407 }
Leon 0:694620b4c620 408 period_stepper1=delay_1-3;
Leon 1:163fa4a925d9 409 if (tick_attached_elbow==0) {
Leon 0:694620b4c620 410 pulse_stepper1.attach_us(&clk_stepper1, period_stepper1);
Leon 1:163fa4a925d9 411 tick_attached_elbow=1;
Leon 0:694620b4c620 412 }
Leon 0:694620b4c620 413 } else {
Leon 1:163fa4a925d9 414 tick_attached_elbow=0;
Leon 0:694620b4c620 415 pulse_stepper1.detach();
Leon 0:694620b4c620 416 }
Leon 0:694620b4c620 417
Leon 1:163fa4a925d9 418 if (err_a_shoulder!=0) {
Leon 1:163fa4a925d9 419 delay_2=delay_ms*1000/err_a_shoulder;
Leon 0:694620b4c620 420 if (delay_2<250) {
Leon 0:694620b4c620 421 pc.printf("X:%f Y:%f\r\n",X_pos, Y_pos);
Leon 0:694620b4c620 422 printf("delay2 : %f\r\n",delay_2);
Leon 1:163fa4a925d9 423 printf("a_shoulder:%d a_shoulder_target:%d err_epaul : %d\r\n\r\n",a_shoulder, a_shoulder_target, err_a_shoulder);
Leon 0:694620b4c620 424 delay_2=250;
Leon 0:694620b4c620 425 led_err=1;
Leon 0:694620b4c620 426 led_err=0;
Leon 0:694620b4c620 427 }
Leon 0:694620b4c620 428 period_stepper2=delay_2-3;
Leon 1:163fa4a925d9 429 if (tick_attached_shoulder==0) {
Leon 0:694620b4c620 430 pulse_stepper2.attach_us(&clk_stepper2, period_stepper2);
Leon 1:163fa4a925d9 431 tick_attached_shoulder=1;
Leon 0:694620b4c620 432 }
Leon 0:694620b4c620 433 } else {
Leon 1:163fa4a925d9 434 tick_attached_shoulder=0;
Leon 0:694620b4c620 435 pulse_stepper2.detach();
Leon 0:694620b4c620 436 }
Leon 0:694620b4c620 437 wait_ms(delay_ms);
Leon 0:694620b4c620 438 }
Leon 0:694620b4c620 439
Leon 1:163fa4a925d9 440 void goto_2D_short (float X_target, float Y_target, int delay_ms)
Leon 0:694620b4c620 441 {
Leon 1:163fa4a925d9 442 float arm_extension, deltax, deltay, alpha_D, arad_elbow_target, arad_shoulder_target, setpoint_elbow, setpoint_shoulder, error_elbow, error_shoulder;
Leon 0:694620b4c620 443 int delay_loc;
Leon 1:163fa4a925d9 444 // simplified inverse kinematics
Leon 1:163fa4a925d9 445 if (X_target > 360) {
Leon 1:163fa4a925d9 446 X_target=360;
Leon 0:694620b4c620 447 }
Leon 1:163fa4a925d9 448 if (X_target < 0) {
Leon 1:163fa4a925d9 449 X_target=0;
Leon 0:694620b4c620 450 }
Leon 1:163fa4a925d9 451 if (Y_target > 145) {
Leon 1:163fa4a925d9 452 Y_target=145; //110
Leon 0:694620b4c620 453 }
Leon 1:163fa4a925d9 454 if (Y_target < 0) {
Leon 1:163fa4a925d9 455 Y_target=0;
Leon 0:694620b4c620 456 }
Leon 1:163fa4a925d9 457 deltax=X_target-180;
Leon 1:163fa4a925d9 458 deltay=235-Y_target;//220
Leon 1:163fa4a925d9 459 arm_extension=sqrt((deltax*deltax)+(deltay*deltay));
Leon 1:163fa4a925d9 460 arad_elbow_target=2*acos(arm_extension/320);
Leon 0:694620b4c620 461 alpha_D=atan(deltax/deltay);
Leon 0:694620b4c620 462
Leon 1:163fa4a925d9 463 //conversion to steps
Leon 1:163fa4a925d9 464 if (tool==0) { //pen
Leon 1:163fa4a925d9 465 arad_shoulder_target=alpha_D+(arad_elbow_target/2);
Leon 1:163fa4a925d9 466 setpoint_shoulder = arad_shoulder_target*1120.5;
Leon 1:163fa4a925d9 467 setpoint_elbow = setpoint_shoulder/4.4 -arad_elbow_target*1120.5;
Leon 0:694620b4c620 468 } else {
Leon 1:163fa4a925d9 469 arad_shoulder_target=alpha_D-(arad_elbow_target/2); //gomme
Leon 1:163fa4a925d9 470 setpoint_shoulder = arad_shoulder_target*1120.5;
Leon 1:163fa4a925d9 471 setpoint_elbow = setpoint_shoulder/4.4 +arad_elbow_target*1120.5-850;
Leon 0:694620b4c620 472 }
Leon 0:694620b4c620 473
Leon 0:694620b4c620 474 if (delay_ms==0) {
Leon 1:163fa4a925d9 475 error_elbow=abs(setpoint_elbow-a_elbow);
Leon 1:163fa4a925d9 476 error_shoulder=abs(setpoint_shoulder-a_shoulder);
Leon 1:163fa4a925d9 477 if (error_elbow>error_shoulder) {
Leon 1:163fa4a925d9 478 delay_loc=0.75*error_elbow;
Leon 0:694620b4c620 479 } else {
Leon 1:163fa4a925d9 480 delay_loc=0.75*error_shoulder;
Leon 0:694620b4c620 481 }
Leon 1:163fa4a925d9 482 setpos_ang (setpoint_elbow, setpoint_shoulder, delay_loc);
Leon 1:163fa4a925d9 483 X_pos=X_target;
Leon 1:163fa4a925d9 484 Y_pos=Y_target;
Leon 0:694620b4c620 485 } else {
Leon 1:163fa4a925d9 486 setpos_ang (setpoint_elbow, setpoint_shoulder, delay_ms);
Leon 1:163fa4a925d9 487 X_pos=X_target;
Leon 1:163fa4a925d9 488 Y_pos=Y_target;
Leon 0:694620b4c620 489 }
Leon 0:694620b4c620 490 }
Leon 0:694620b4c620 491
Leon 1:163fa4a925d9 492 void goto_2D_line (float X_target, float Y_target)
Leon 0:694620b4c620 493 {
Leon 0:694620b4c620 494 float delta_x, delta_y, distance, x_decomp, y_decomp;
Leon 1:163fa4a925d9 495 int step_nb, i;
Leon 1:163fa4a925d9 496 delta_x=X_target-X_pos;
Leon 1:163fa4a925d9 497 delta_y=Y_target-Y_pos;
Leon 0:694620b4c620 498 distance=sqrt((delta_x*delta_x)+(delta_y*delta_y));
Leon 1:163fa4a925d9 499 step_nb = ceil (distance*1);// 1 step = 1 mm
Leon 1:163fa4a925d9 500 delta_x=delta_x/step_nb;
Leon 1:163fa4a925d9 501 delta_y=delta_y/step_nb;
Leon 0:694620b4c620 502 x_decomp=X_pos;
Leon 0:694620b4c620 503 y_decomp=Y_pos;
Leon 1:163fa4a925d9 504 for (i=1; i<=step_nb; i++) {
Leon 0:694620b4c620 505 x_decomp=x_decomp+delta_x;
Leon 0:694620b4c620 506 y_decomp=y_decomp+delta_y;
Leon 1:163fa4a925d9 507 if(accurate==1) {
Leon 0:694620b4c620 508 goto_2D_short (x_decomp, y_decomp, 10+70*potard_1.read()); //12
Leon 1:163fa4a925d9 509 } else if (accurate==2) {
Leon 0:694620b4c620 510 goto_2D_short (x_decomp, y_decomp,13);
Leon 0:694620b4c620 511 } else {
Leon 0:694620b4c620 512 goto_2D_short (x_decomp, y_decomp, 9);
Leon 0:694620b4c620 513 }
Leon 0:694620b4c620 514 }
Leon 0:694620b4c620 515 tick_detach();
Leon 0:694620b4c620 516 }
Leon 0:694620b4c620 517
Leon 1:163fa4a925d9 518 void circle_2D(float X_centre, float Y_centre, float radius, float angle_deb, float angle_fin)
Leon 0:694620b4c620 519 {
Leon 1:163fa4a925d9 520 float step_angle, current_angle, x_decomp, y_decomp, delta_x, delta_y, distance;
Leon 1:163fa4a925d9 521 int step_nb, i;
Leon 1:163fa4a925d9 522 step_nb=1*ceil (radius * abs (angle_fin-angle_deb));
Leon 1:163fa4a925d9 523 step_angle=(angle_fin-angle_deb)/step_nb;
Leon 1:163fa4a925d9 524 current_angle=angle_deb;
Leon 1:163fa4a925d9 525 x_decomp=X_centre + radius*cos (current_angle);
Leon 1:163fa4a925d9 526 y_decomp=Y_centre + radius*sin (current_angle);
Leon 0:694620b4c620 527 delta_x=x_decomp-X_pos;
Leon 0:694620b4c620 528 delta_y=y_decomp-Y_pos;
Leon 0:694620b4c620 529 distance=sqrt((delta_x*delta_x)+(delta_y*delta_y));
Leon 0:694620b4c620 530 if (distance>2) {
Leon 1:163fa4a925d9 531 set_wrist(2);
Leon 0:694620b4c620 532 goto_2D_line (x_decomp, y_decomp);
Leon 0:694620b4c620 533 }
Leon 1:163fa4a925d9 534 set_wrist(1);
Leon 1:163fa4a925d9 535 for (i=1; i<=step_nb; i++) {
Leon 1:163fa4a925d9 536 current_angle=current_angle+step_angle;
Leon 1:163fa4a925d9 537 x_decomp=X_centre + radius*cos (current_angle);
Leon 1:163fa4a925d9 538 y_decomp=Y_centre + radius*sin (current_angle);
Leon 0:694620b4c620 539 goto_2D_short (x_decomp, y_decomp, 10+70*potard_1.read());
Leon 0:694620b4c620 540 }
Leon 0:694620b4c620 541 tick_detach();
Leon 0:694620b4c620 542 }
Leon 0:694620b4c620 543
Leon 1:163fa4a925d9 544 void set_tool(int tool_loc, float X_loc, float Y_loc)
Leon 0:694620b4c620 545 {
Leon 1:163fa4a925d9 546 if ((tool_loc==1)&&(tool==0)) {//eraser
Leon 1:163fa4a925d9 547 set_wrist(3);
Leon 1:163fa4a925d9 548 tool=1;
Leon 0:694620b4c620 549 goto_2D_short (X_loc, Y_loc,0);//2000
Leon 0:694620b4c620 550 tick_detach();
Leon 1:163fa4a925d9 551 set_wrist(4);
Leon 0:694620b4c620 552 wait_ms(200);
Leon 1:163fa4a925d9 553 } else if ((tool_loc==0)&&(tool==1)) {
Leon 1:163fa4a925d9 554 set_wrist(3);
Leon 1:163fa4a925d9 555 tool=0;
Leon 0:694620b4c620 556 goto_2D_short (X_loc, Y_loc,0);//2000
Leon 0:694620b4c620 557 tick_detach();
Leon 1:163fa4a925d9 558 set_wrist(2);
Leon 0:694620b4c620 559 wait_ms(200);
Leon 1:163fa4a925d9 560 } else if ((tool_loc==1)&&(tool==1)) {
Leon 1:163fa4a925d9 561 set_wrist(4);
Leon 0:694620b4c620 562 }
Leon 0:694620b4c620 563 }
Leon 0:694620b4c620 564
Leon 1:163fa4a925d9 565 void scr_print_single_char(char charact, float size)
Leon 0:694620b4c620 566 {
Leon 1:163fa4a925d9 567 int s_end_char=0;
Leon 0:694620b4c620 568 int i_instr=0;
Leon 1:163fa4a925d9 569 int x_temp, y_temp, charact_loc;
Leon 0:694620b4c620 570 float X_local, Y_local;
Leon 1:163fa4a925d9 571 float radius_loc, angle_deb_loc, angle_fin_loc;
Leon 1:163fa4a925d9 572 charact_loc=charact;
Leon 1:163fa4a925d9 573 //printf ("char: %d", charact+32);
Leon 1:163fa4a925d9 574 if (charact_loc<0) {
Leon 1:163fa4a925d9 575 charact_loc=charact+256;
Leon 0:694620b4c620 576 }
Leon 1:163fa4a925d9 577 //printf ("char: %d\r\n", charact_loc+32);
Leon 1:163fa4a925d9 578 set_tool(0,X_print,Y_print);
Leon 1:163fa4a925d9 579 while (s_end_char==0) {
Leon 1:163fa4a925d9 580 switch (font[charact_loc][i_instr]) {
Leon 1:163fa4a925d9 581 case 2 : // goto pen raised
Leon 1:163fa4a925d9 582 //wrist_servo_setpoint=raised_pen_servopos;
Leon 1:163fa4a925d9 583 set_wrist(2);
Leon 0:694620b4c620 584 //wait_ms(100);
Leon 1:163fa4a925d9 585 X_local=X_print+size*(font[charact_loc][i_instr+1]);
Leon 1:163fa4a925d9 586 y_temp=font[charact_loc][i_instr+2];
Leon 0:694620b4c620 587 if (y_temp>127) {
Leon 0:694620b4c620 588 y_temp=y_temp-256;
Leon 0:694620b4c620 589 }
Leon 1:163fa4a925d9 590 Y_local=Y_print+size*y_temp;
Leon 0:694620b4c620 591 goto_2D_line(X_local, Y_local);
Leon 0:694620b4c620 592 i_instr=i_instr+3;
Leon 0:694620b4c620 593 break;
Leon 1:163fa4a925d9 594 case 3 : // line pen laid
Leon 1:163fa4a925d9 595 set_wrist(1);
Leon 1:163fa4a925d9 596 //if (wrist_servo_setpoint!=laid_pen_servopos) {
Leon 1:163fa4a925d9 597 // wrist_servo_setpoint=laid_pen_servopos;
Leon 0:694620b4c620 598 // wait_ms(100);
Leon 0:694620b4c620 599 //}
Leon 1:163fa4a925d9 600 X_local=X_print+size*(font[charact_loc][i_instr+1]);
Leon 1:163fa4a925d9 601 y_temp=font[charact_loc][i_instr+2];
Leon 0:694620b4c620 602 if (y_temp>127) {
Leon 0:694620b4c620 603 y_temp=y_temp-256;
Leon 0:694620b4c620 604 }
Leon 1:163fa4a925d9 605 Y_local=Y_print+size*y_temp;
Leon 0:694620b4c620 606 goto_2D_line(X_local, Y_local);
Leon 0:694620b4c620 607 i_instr=i_instr+3;
Leon 0:694620b4c620 608 break;
Leon 1:163fa4a925d9 609 case 4 : // circle
Leon 1:163fa4a925d9 610 //X_local=X_print+size*(font[charact_loc][i_instr+1]);
Leon 1:163fa4a925d9 611 x_temp=font[charact_loc][i_instr+1];
Leon 0:694620b4c620 612 if (x_temp>127) {
Leon 0:694620b4c620 613 x_temp=x_temp-256;
Leon 0:694620b4c620 614 }
Leon 1:163fa4a925d9 615 X_local=X_print+size*x_temp;
Leon 1:163fa4a925d9 616 y_temp=font[charact_loc][i_instr+2];
Leon 0:694620b4c620 617 if (y_temp>127) {
Leon 0:694620b4c620 618 y_temp=y_temp-256;
Leon 0:694620b4c620 619 }
Leon 1:163fa4a925d9 620 Y_local=Y_print+size*y_temp;
Leon 1:163fa4a925d9 621 radius_loc=size*(font[charact_loc][i_instr+3])/2;
Leon 1:163fa4a925d9 622 angle_deb_loc=(0.3927*font[charact_loc][i_instr+4])-3.1416;
Leon 1:163fa4a925d9 623 angle_fin_loc=(0.3927*font[charact_loc][i_instr+5])-3.1416;
Leon 0:694620b4c620 624 //pc.printf("angles: %f %f \r\n", angle_deb_loc, angle_fin_loc);
Leon 1:163fa4a925d9 625 circle_2D(X_local, Y_local, radius_loc, angle_deb_loc, angle_fin_loc);
Leon 0:694620b4c620 626 i_instr=i_instr+6;
Leon 0:694620b4c620 627 break;
Leon 1:163fa4a925d9 628 case 5 : // end
Leon 1:163fa4a925d9 629 s_end_char=1;
Leon 1:163fa4a925d9 630 X_print=X_print+size*(font[charact_loc][i_instr+1]);
Leon 1:163fa4a925d9 631 //wrist_servo_setpoint=raised_pen_servopos;
Leon 1:163fa4a925d9 632 set_wrist(2);
Leon 0:694620b4c620 633 //wait_ms(50);
Leon 0:694620b4c620 634 break;
Leon 0:694620b4c620 635 case 6 : // point
Leon 1:163fa4a925d9 636 X_local=X_print+size*(font[charact_loc][i_instr+1]);
Leon 1:163fa4a925d9 637 Y_local=Y_print+size*(font[charact_loc][i_instr+2]);
Leon 1:163fa4a925d9 638 set_wrist(2);
Leon 0:694620b4c620 639 goto_2D_line(X_local, Y_local);
Leon 1:163fa4a925d9 640 set_wrist(1);
Leon 0:694620b4c620 641 goto_2D_line(X_local, Y_local);
Leon 1:163fa4a925d9 642 set_wrist(2);
Leon 0:694620b4c620 643 i_instr=i_instr+3;
Leon 0:694620b4c620 644 break;
Leon 0:694620b4c620 645 }
Leon 0:694620b4c620 646 }
Leon 0:694620b4c620 647 }
Leon 0:694620b4c620 648
Leon 1:163fa4a925d9 649 void scr_print_single_char_fictive(char charact, float size)
Leon 0:694620b4c620 650 {
Leon 1:163fa4a925d9 651 int s_end_char=0;
Leon 0:694620b4c620 652 int i_instr=0;
Leon 1:163fa4a925d9 653 int x_temp, y_temp, charact_loc;
Leon 0:694620b4c620 654 float X_local, Y_local;
Leon 1:163fa4a925d9 655 float radius_loc, angle_deb_loc, angle_fin_loc;
Leon 1:163fa4a925d9 656 charact_loc=charact;
Leon 1:163fa4a925d9 657 //printf ("char: %d", charact+32);
Leon 1:163fa4a925d9 658 if (charact_loc<0) {
Leon 1:163fa4a925d9 659 charact_loc=charact+256;
Leon 0:694620b4c620 660 }
Leon 1:163fa4a925d9 661 //printf ("char fictive: %d\r\n", charact_loc+32);
Leon 1:163fa4a925d9 662 while (s_end_char==0) {
Leon 1:163fa4a925d9 663 switch (font[charact_loc][i_instr]) {
Leon 1:163fa4a925d9 664 case 2 : // goto pen raised
Leon 0:694620b4c620 665 i_instr=i_instr+3;
Leon 0:694620b4c620 666 break;
Leon 1:163fa4a925d9 667 case 3 : // line pen pose
Leon 0:694620b4c620 668 i_instr=i_instr+3;
Leon 0:694620b4c620 669 break;
Leon 1:163fa4a925d9 670 case 4 : // circle
Leon 0:694620b4c620 671 i_instr=i_instr+6;
Leon 0:694620b4c620 672 break;
Leon 1:163fa4a925d9 673 case 5 : // end
Leon 1:163fa4a925d9 674 s_end_char=1;
Leon 1:163fa4a925d9 675 X_fictive=X_fictive+size*(font[charact_loc][i_instr+1]);
Leon 0:694620b4c620 676 break;
Leon 0:694620b4c620 677 case 6 : // point
Leon 0:694620b4c620 678 i_instr=i_instr+3;
Leon 0:694620b4c620 679 break;
Leon 0:694620b4c620 680 }
Leon 0:694620b4c620 681 }
Leon 0:694620b4c620 682 }
Leon 0:694620b4c620 683
Leon 1:163fa4a925d9 684 void erase(float Xl1, float Yl1, float Xl2, float Yl2)
Leon 0:694620b4c620 685 {
Leon 0:694620b4c620 686 float distance_Y,pas, Y_loc;
Leon 1:163fa4a925d9 687 int i, step_nb, tool_old_loc;
Leon 1:163fa4a925d9 688 tool_old_loc=tool;
Leon 1:163fa4a925d9 689 set_tool(1, Xl1, Yl1);
Leon 0:694620b4c620 690 goto_2D_line(Xl1,Yl1);
Leon 1:163fa4a925d9 691 set_wrist(5);
Leon 0:694620b4c620 692 distance_Y=Yl2-Yl1;
Leon 0:694620b4c620 693 Y_loc=Yl1;
Leon 1:163fa4a925d9 694
Leon 0:694620b4c620 695 while (Y_loc<Yl2) {
Leon 0:694620b4c620 696 goto_2D_line(Xl1, Y_loc);
Leon 0:694620b4c620 697 goto_2D_line(Xl2, Y_loc);
Leon 0:694620b4c620 698 Y_loc=Y_loc+5;
Leon 0:694620b4c620 699 goto_2D_line(Xl2, Y_loc);
Leon 0:694620b4c620 700 goto_2D_line(Xl1, Y_loc);
Leon 0:694620b4c620 701 Y_loc=Y_loc+5;
Leon 0:694620b4c620 702 }
Leon 1:163fa4a925d9 703 set_wrist(4);
Leon 0:694620b4c620 704 }
Leon 0:694620b4c620 705
Leon 0:694620b4c620 706
Leon 1:163fa4a925d9 707 void scrib_print_hour(void)
Leon 0:694620b4c620 708 {
Leon 0:694620b4c620 709 char texte_heure[10];
Leon 0:694620b4c620 710 char texte_minute[10];
Leon 1:163fa4a925d9 711 int tool_bk,accurate_bk, difference;
Leon 1:163fa4a925d9 712 int wrist_servo_setpoint_bk;
Leon 0:694620b4c620 713 float X_backup, Y_backup,X_pos_bk, Y_pos_bk;
Leon 0:694620b4c620 714 struct tm * timeinfo;
Leon 0:694620b4c620 715 time_t seconds = time(NULL);
Leon 0:694620b4c620 716 timeinfo = localtime (&seconds);
Leon 0:694620b4c620 717 strftime(texte_heure,10,"%H",timeinfo);
Leon 0:694620b4c620 718 strftime(texte_minute,10,"%M",timeinfo);
Leon 0:694620b4c620 719 X_backup=X_print;
Leon 0:694620b4c620 720 Y_backup=Y_print;
Leon 0:694620b4c620 721 X_pos_bk=X_pos;
Leon 0:694620b4c620 722 Y_pos_bk=Y_pos;
Leon 1:163fa4a925d9 723 tool_bk=tool;
Leon 1:163fa4a925d9 724 accurate_bk=accurate;
Leon 0:694620b4c620 725 difference=0;
Leon 1:163fa4a925d9 726 wrist_servo_setpoint_bk=wrist_servo_setpoint;
Leon 1:163fa4a925d9 727 if (strcmp(texte_minute, minute_txt_back)!=0) {
Leon 1:163fa4a925d9 728 erase(325,107,359,138);
Leon 0:694620b4c620 729 X_print=330;
Leon 0:694620b4c620 730 Y_print=110;
Leon 1:163fa4a925d9 731 scr_print_single_char(texte_minute[0]-32,glob_size);
Leon 0:694620b4c620 732 X_print=345;
Leon 1:163fa4a925d9 733 scr_print_single_char(texte_minute[1]-32,glob_size);
Leon 1:163fa4a925d9 734 strcpy(minute_txt_back,texte_minute);
Leon 0:694620b4c620 735 difference=1;
Leon 0:694620b4c620 736 }
Leon 1:163fa4a925d9 737 if (strcmp(texte_heure, hour_txt_back)!=0) {
Leon 1:163fa4a925d9 738 erase(267,107,304,135);
Leon 0:694620b4c620 739 X_print=275;
Leon 0:694620b4c620 740 Y_print=110;
Leon 1:163fa4a925d9 741 scr_print_single_char(texte_heure[0]-32,glob_size);
Leon 0:694620b4c620 742 X_print=290;
Leon 1:163fa4a925d9 743 scr_print_single_char(texte_heure[1]-32,glob_size);
Leon 1:163fa4a925d9 744 strcpy(hour_txt_back,texte_heure);
Leon 0:694620b4c620 745 difference=1;
Leon 0:694620b4c620 746 }
Leon 1:163fa4a925d9 747 if ((tool_bk==1)&&(difference==1)) {
Leon 1:163fa4a925d9 748 set_tool(1,X_pos_bk,Y_pos_bk);
Leon 0:694620b4c620 749 }
Leon 0:694620b4c620 750
Leon 0:694620b4c620 751 X_print=X_backup;
Leon 0:694620b4c620 752 Y_print=Y_backup;
Leon 1:163fa4a925d9 753
Leon 1:163fa4a925d9 754 wrist_servo_setpoint=wrist_servo_setpoint_bk;
Leon 1:163fa4a925d9 755 accurate=accurate_bk;
Leon 0:694620b4c620 756 }
Leon 0:694620b4c620 757
Leon 1:163fa4a925d9 758 void erase_line (int line_nb)
Leon 0:694620b4c620 759 {
Leon 0:694620b4c620 760 float i;
Leon 1:163fa4a925d9 761 //set_tool(1);
Leon 0:694620b4c620 762 switch (line_nb) {
Leon 0:694620b4c620 763 case 1 :
Leon 1:163fa4a925d9 764 set_tool(1,0,112);
Leon 1:163fa4a925d9 765
Leon 1:163fa4a925d9 766 set_wrist(5);
Leon 0:694620b4c620 767 for (i=0; i<31; i=i+10.6) {
Leon 0:694620b4c620 768 goto_2D_line(0,112+i);//104
Leon 1:163fa4a925d9 769 //scrib_print_hour();
Leon 0:694620b4c620 770 goto_2D_line(120,116+i);//113
Leon 1:163fa4a925d9 771 scrib_print_hour();
Leon 0:694620b4c620 772 goto_2D_line(240,109+i);//104
Leon 1:163fa4a925d9 773 scrib_print_hour();
Leon 1:163fa4a925d9 774
Leon 0:694620b4c620 775 goto_2D_line(265,108+i);
Leon 1:163fa4a925d9 776 scrib_print_hour();
Leon 0:694620b4c620 777 goto_2D_line(265,108+i+5.3);
Leon 1:163fa4a925d9 778 scrib_print_hour();
Leon 0:694620b4c620 779 goto_2D_line(240,109+i+5.3);
Leon 1:163fa4a925d9 780 scrib_print_hour();
Leon 0:694620b4c620 781 goto_2D_line(120,116+i+5.3);
Leon 1:163fa4a925d9 782 scrib_print_hour();
Leon 0:694620b4c620 783 goto_2D_line(0,112+i+5.3);
Leon 1:163fa4a925d9 784 //scrib_print_hour();
Leon 0:694620b4c620 785 }
Leon 0:694620b4c620 786 break;
Leon 0:694620b4c620 787 case 2 :
Leon 1:163fa4a925d9 788 set_tool(1,0,70);
Leon 1:163fa4a925d9 789
Leon 1:163fa4a925d9 790 set_wrist(5);
Leon 0:694620b4c620 791 for (i=0; i<31; i=i+10.6) { //8
Leon 0:694620b4c620 792 goto_2D_line(0,76+i);//104
Leon 1:163fa4a925d9 793 //scrib_print_hour();
Leon 0:694620b4c620 794 goto_2D_line(120,81+i);//113
Leon 1:163fa4a925d9 795 scrib_print_hour();
Leon 0:694620b4c620 796 goto_2D_line(240,78+i);//104
Leon 1:163fa4a925d9 797 scrib_print_hour();
Leon 0:694620b4c620 798 goto_2D_line(360,72+i);
Leon 1:163fa4a925d9 799 scrib_print_hour();
Leon 0:694620b4c620 800 goto_2D_line(360,72+i+5.3);
Leon 1:163fa4a925d9 801 scrib_print_hour();
Leon 0:694620b4c620 802 goto_2D_line(240,78+i+5.3);
Leon 1:163fa4a925d9 803 scrib_print_hour();
Leon 0:694620b4c620 804 goto_2D_line(120,81+i+5.3);
Leon 1:163fa4a925d9 805 scrib_print_hour();
Leon 0:694620b4c620 806 goto_2D_line(0,76+i+5.3);//4
Leon 1:163fa4a925d9 807 //scrib_print_hour();
Leon 0:694620b4c620 808 }
Leon 0:694620b4c620 809 break;
Leon 0:694620b4c620 810 case 3 :
Leon 1:163fa4a925d9 811
Leon 1:163fa4a925d9 812 set_tool(1,0,37);
Leon 1:163fa4a925d9 813
Leon 1:163fa4a925d9 814 set_wrist(5);
Leon 1:163fa4a925d9 815 wrist_servo_setpoint=745;
Leon 0:694620b4c620 816 for (i=0; i<31; i=i+10.6) {
Leon 0:694620b4c620 817 goto_2D_line(0,39+i);//104
Leon 1:163fa4a925d9 818 //scrib_print_hour();
Leon 0:694620b4c620 819 goto_2D_line(120,46+i);//113
Leon 1:163fa4a925d9 820 scrib_print_hour();
Leon 0:694620b4c620 821 goto_2D_line(240,47+i);//104
Leon 1:163fa4a925d9 822 scrib_print_hour();
Leon 0:694620b4c620 823 goto_2D_line(360,44+i);
Leon 1:163fa4a925d9 824 scrib_print_hour();
Leon 0:694620b4c620 825 goto_2D_line(360,44+i+5.3);
Leon 1:163fa4a925d9 826 scrib_print_hour();
Leon 0:694620b4c620 827 goto_2D_line(240,47+i+5.3);
Leon 1:163fa4a925d9 828 scrib_print_hour();
Leon 0:694620b4c620 829 goto_2D_line(120,46+i+5.3);
Leon 1:163fa4a925d9 830 scrib_print_hour();
Leon 0:694620b4c620 831 goto_2D_line(0,39+i+5.3);
Leon 0:694620b4c620 832 }
Leon 0:694620b4c620 833 break;
Leon 0:694620b4c620 834
Leon 0:694620b4c620 835 case 4 :
Leon 1:163fa4a925d9 836
Leon 1:163fa4a925d9 837 set_tool(1,0,5);
Leon 1:163fa4a925d9 838
Leon 1:163fa4a925d9 839 set_wrist(5);
Leon 1:163fa4a925d9 840 wrist_servo_setpoint=740;
Leon 0:694620b4c620 841 for (i=0; i<31; i=i+10.6) {
Leon 0:694620b4c620 842 goto_2D_line(0,5+i);//104
Leon 1:163fa4a925d9 843 //scrib_print_hour();
Leon 0:694620b4c620 844 goto_2D_line(120,12+i);//113
Leon 1:163fa4a925d9 845 scrib_print_hour();
Leon 0:694620b4c620 846 goto_2D_line(240,12+i);//104
Leon 1:163fa4a925d9 847 scrib_print_hour();
Leon 0:694620b4c620 848 goto_2D_line(360,8+i);
Leon 1:163fa4a925d9 849 scrib_print_hour();
Leon 0:694620b4c620 850 goto_2D_line(360,8+i+5.3);
Leon 1:163fa4a925d9 851 scrib_print_hour();
Leon 0:694620b4c620 852 goto_2D_line(240,12+i+5.3);
Leon 1:163fa4a925d9 853 scrib_print_hour();
Leon 0:694620b4c620 854 goto_2D_line(120,12+i+5.3);
Leon 1:163fa4a925d9 855 scrib_print_hour();
Leon 0:694620b4c620 856 goto_2D_line(0,5+i+5.3);
Leon 0:694620b4c620 857 }
Leon 0:694620b4c620 858 break;
Leon 0:694620b4c620 859 }
Leon 1:163fa4a925d9 860 set_wrist(4);
Leon 0:694620b4c620 861
Leon 0:694620b4c620 862 }
Leon 0:694620b4c620 863
Leon 1:163fa4a925d9 864 void scrib_line_return(void)
Leon 0:694620b4c620 865 {
Leon 1:163fa4a925d9 866 switch (line) {
Leon 0:694620b4c620 867 case 1:
Leon 1:163fa4a925d9 868 line_filling_st[1]=1;
Leon 1:163fa4a925d9 869 if (line_filling_st[2]==1) {
Leon 1:163fa4a925d9 870 erase_line(2);
Leon 0:694620b4c620 871 }
Leon 0:694620b4c620 872 X_print=1;
Leon 0:694620b4c620 873 Y_print=77;
Leon 1:163fa4a925d9 874 line=2;
Leon 0:694620b4c620 875 break;
Leon 0:694620b4c620 876 case 2:
Leon 1:163fa4a925d9 877 line_filling_st[2]=1;
Leon 1:163fa4a925d9 878 if (line_filling_st[3]==1) {
Leon 1:163fa4a925d9 879 erase_line(3);
Leon 0:694620b4c620 880 }
Leon 0:694620b4c620 881 X_print=1;
Leon 0:694620b4c620 882 Y_print=44;
Leon 1:163fa4a925d9 883 line=3;
Leon 0:694620b4c620 884 break;
Leon 0:694620b4c620 885 case 3:
Leon 1:163fa4a925d9 886 line_filling_st[3]=1;
Leon 1:163fa4a925d9 887 if (line_filling_st[4]==1) {
Leon 1:163fa4a925d9 888 erase_line(4);
Leon 0:694620b4c620 889 }
Leon 0:694620b4c620 890 X_print=1;
Leon 0:694620b4c620 891 Y_print=11;
Leon 1:163fa4a925d9 892 line=4;
Leon 0:694620b4c620 893 break;
Leon 0:694620b4c620 894 case 4:
Leon 1:163fa4a925d9 895 line_filling_st[4]=1;
Leon 1:163fa4a925d9 896 if (line_filling_st[1]==1) {
Leon 1:163fa4a925d9 897 erase_line(1);
Leon 0:694620b4c620 898 }
Leon 0:694620b4c620 899 X_print=1;
Leon 0:694620b4c620 900 Y_print=110;
Leon 1:163fa4a925d9 901 line=1;
Leon 0:694620b4c620 902 break;
Leon 0:694620b4c620 903 }
Leon 0:694620b4c620 904 }
Leon 0:694620b4c620 905
Leon 1:163fa4a925d9 906 void scrib_print_hour_initial(void)
Leon 0:694620b4c620 907 {
Leon 0:694620b4c620 908 char texte_heure[10];
Leon 0:694620b4c620 909
Leon 0:694620b4c620 910 struct tm * timeinfo;
Leon 0:694620b4c620 911 time_t seconds = time(NULL);
Leon 0:694620b4c620 912 timeinfo = localtime (&seconds);
Leon 1:163fa4a925d9 913 strftime(hour_txt_back,10,"%H",timeinfo);
Leon 1:163fa4a925d9 914 strftime(minute_txt_back,10,"%M",timeinfo);
Leon 0:694620b4c620 915 X_print=275;
Leon 0:694620b4c620 916 Y_print=110;
Leon 1:163fa4a925d9 917 scr_print_single_char(hour_txt_back[0]-32,glob_size);
Leon 0:694620b4c620 918 X_print=290;
Leon 1:163fa4a925d9 919 scr_print_single_char(hour_txt_back[1]-32,glob_size);
Leon 0:694620b4c620 920 X_print=310;
Leon 1:163fa4a925d9 921 scr_print_single_char(104-32,glob_size);
Leon 0:694620b4c620 922 X_print=330;
Leon 1:163fa4a925d9 923 scr_print_single_char(minute_txt_back[0]-32,glob_size);
Leon 0:694620b4c620 924 X_print=345;
Leon 1:163fa4a925d9 925 scr_print_single_char(minute_txt_back[1]-32,glob_size);
Leon 0:694620b4c620 926 }
Leon 0:694620b4c620 927
Leon 0:694620b4c620 928
Leon 1:163fa4a925d9 929 void scrib_print_string(char* chaine)
Leon 0:694620b4c620 930 {
Leon 0:694620b4c620 931 int i=0;
Leon 1:163fa4a925d9 932 int i_fictive=0;
Leon 1:163fa4a925d9 933 int line_feed_loc=0;
Leon 0:694620b4c620 934 int rupt=0;
Leon 0:694620b4c620 935 while (chaine[i]!=0) {
Leon 1:163fa4a925d9 936 scrib_print_hour();
Leon 1:163fa4a925d9 937 line_feed_loc=0;
Leon 0:694620b4c620 938 if (chaine[i]>31) {
Leon 1:163fa4a925d9 939 scr_print_single_char(chaine[i]-32,glob_size);
Leon 0:694620b4c620 940 } else if (chaine[i]==10) {
Leon 1:163fa4a925d9 941 scrib_line_return();
Leon 1:163fa4a925d9 942
Leon 0:694620b4c620 943 }
Leon 1:163fa4a925d9 944 if ((X_print>345)||((line==1)&&(X_print>270))) {
Leon 1:163fa4a925d9 945 scrib_line_return();
Leon 1:163fa4a925d9 946
Leon 1:163fa4a925d9 947 line_feed_loc=1;
Leon 0:694620b4c620 948 }
Leon 1:163fa4a925d9 949 if ((chaine[i]==32)&&(line_feed_loc==0)) { //determine
Leon 1:163fa4a925d9 950 X_fictive=X_print;
Leon 1:163fa4a925d9 951 i_fictive=i+1;
Leon 0:694620b4c620 952 rupt=0;
Leon 1:163fa4a925d9 953 while (rupt==0) { //we step X_fictive until the next space char or line feed or endofchar.
Leon 1:163fa4a925d9 954 if (chaine[i_fictive]>31) {
Leon 1:163fa4a925d9 955 scr_print_single_char_fictive(chaine[i_fictive]-32,glob_size);
Leon 0:694620b4c620 956 }
Leon 1:163fa4a925d9 957 if ((X_fictive>345)||((line==1)&&(X_fictive>270))) {
Leon 0:694620b4c620 958 rupt=2;
Leon 0:694620b4c620 959 }
Leon 1:163fa4a925d9 960 if ((chaine[i_fictive]==32)||(chaine[i_fictive]==10)||(chaine[i_fictive]==0)) {
Leon 0:694620b4c620 961 rupt=1;
Leon 0:694620b4c620 962 }
Leon 1:163fa4a925d9 963 i_fictive++;
Leon 0:694620b4c620 964 }
Leon 0:694620b4c620 965 if (rupt==2) {
Leon 1:163fa4a925d9 966 scrib_line_return();
Leon 1:163fa4a925d9 967
Leon 0:694620b4c620 968 }
Leon 0:694620b4c620 969 }
Leon 0:694620b4c620 970 i++;
Leon 0:694620b4c620 971 }
Leon 0:694620b4c620 972 }
Leon 0:694620b4c620 973
Leon 0:694620b4c620 974
Leon 0:694620b4c620 975
Leon 0:694620b4c620 976
Leon 0:694620b4c620 977
Leon 0:694620b4c620 978 int main()
Leon 0:694620b4c620 979 {
Leon 0:694620b4c620 980 FILE* fileid;
Leon 0:694620b4c620 981 int i,j,k;
Leon 0:694620b4c620 982 char montexte[600];
Leon 0:694620b4c620 983 float aaa, bbb, ccc, ddd, eee;
Leon 0:694620b4c620 984
Leon 0:694620b4c620 985 struct tm * timeinfo;
Leon 0:694620b4c620 986
Leon 0:694620b4c620 987 time_t seconds = time(NULL);
Leon 1:163fa4a925d9 988 // ********* initializations *******
Leon 1:163fa4a925d9 989
Leon 0:694620b4c620 990 pc.baud(115200);
Leon 0:694620b4c620 991 pc.printf("bijour\r\n");
Leon 1:163fa4a925d9 992 tick_attached_elbow=0;
Leon 1:163fa4a925d9 993 tick_attached_shoulder=0;
Leon 0:694620b4c620 994 X_print=1;
Leon 0:694620b4c620 995 Y_print=110; //110
Leon 1:163fa4a925d9 996 line=1;
Leon 1:163fa4a925d9 997 accurate=0;
Leon 1:163fa4a925d9 998 tool=0;
Leon 1:163fa4a925d9 999 glob_size=1.5;
Leon 1:163fa4a925d9 1000 servo_call.attach_us(&tick_servo, 20000);
Leon 0:694620b4c620 1001
Leon 1:163fa4a925d9 1002 wrist_servo_setpoint=neutral_pen_servopos;
Leon 0:694620b4c620 1003 wait(1);
Leon 0:694620b4c620 1004
Leon 0:694620b4c620 1005
Leon 0:694620b4c620 1006 fileid=fopen("/local/input.txt", "r");
Leon 0:694620b4c620 1007 fread(montexte,1,500,fileid);
Leon 0:694620b4c620 1008
Leon 0:694620b4c620 1009 montexte[500]=0;
Leon 0:694620b4c620 1010 fclose(fileid);
Leon 0:694620b4c620 1011 for (i=0; i<38; i++) {
Leon 0:694620b4c620 1012 printf(":%d\r\n",montexte[i]);
Leon 0:694620b4c620 1013 }
Leon 0:694620b4c620 1014 pc.printf("Time brut = %d\r\n", seconds);
Leon 0:694620b4c620 1015 pc.printf("Time = %s", ctime(&seconds));
Leon 0:694620b4c620 1016 //set_time(1503151338);//set_time(1336478783);
Leon 0:694620b4c620 1017 timeinfo = localtime (&seconds);
Leon 0:694620b4c620 1018
Leon 0:694620b4c620 1019
Leon 0:694620b4c620 1020 init_stepper();
Leon 1:163fa4a925d9 1021
Leon 1:163fa4a925d9 1022
Leon 0:694620b4c620 1023
Leon 0:694620b4c620 1024 myled=!myled;
Leon 1:163fa4a925d9 1025
Leon 0:694620b4c620 1026 goto_2D_short (0,110, 1000);
Leon 0:694620b4c620 1027 tick_detach();
Leon 1:163fa4a925d9 1028
Leon 0:694620b4c620 1029
Leon 1:163fa4a925d9 1030 scrib_print_hour_initial();
Leon 0:694620b4c620 1031 X_print=1;
Leon 0:694620b4c620 1032 Y_print=110;//110
Leon 0:694620b4c620 1033
Leon 0:694620b4c620 1034 while(1) {
Leon 1:163fa4a925d9 1035 scrib_print_string(montexte);
Leon 0:694620b4c620 1036
Leon 0:694620b4c620 1037 }
Leon 0:694620b4c620 1038 while(1) {}
Leon 0:694620b4c620 1039
Leon 1:163fa4a925d9 1040 set_wrist(3);
Leon 1:163fa4a925d9 1041
Leon 1:163fa4a925d9 1042 while(1) {}
Leon 1:163fa4a925d9 1043
Leon 1:163fa4a925d9 1044 a_elbow=0;
Leon 1:163fa4a925d9 1045 a_shoulder=0;
Leon 0:694620b4c620 1046 aaa=1000;
Leon 0:694620b4c620 1047 bbb=0;
Leon 0:694620b4c620 1048
Leon 0:694620b4c620 1049 }