dual-arm-plotter/main.cpp at master · d2h10s/dual-arm-plotter · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
#include "mbed.h"
#include "protocol.h"

using namespace std;

#define TX D8 // transmit pin name
#define RX D2 // recieve pin name
#define EN D3 // enable pin name

const float
    Y = 30,
    L = 20.8,
    OFFSET1 = 10,
    OFFSET2 = 20.5,
    PI = atan(1.f) * 4.f;

#define delay_us wait_us(M1.delay + M2.delay)
//#define comp(str) (!strcmp(mode.c_str(), str))
//void communication();
void goto_xy(float, float);

DM50 M2(0x01, TX, RX, EN);
DM50 M1(0x02, TX, RX, EN);
Serial PC(USBTX, USBRX, 115200);
//RS485 Tx(D8), RX(D2), enable(D3)

DigitalIn b1(BUTTON1);
DigitalOut led(LED1);

// + positoin means CW
int main(){
    int i = 0;
    printf("===================program on====================\n");
    M1.read_encoder();
    M2.read_encoder();
    M1.init_zero(19);
    M2.init_zero(-24);
    M1.PC1(0);
    M2.PC1(0);
    delay_us;
    while(b1);
    led = 1;

    while(true){
        goto_xy(0,  0);
        while(b1);
        goto_xy(15, 0);
        while(b1);
        goto_xy(30, 0);
        while(b1);
        goto_xy(0,  10);
        while(b1);
        goto_xy(15, 10);
        while(b1);
        goto_xy(30, 10);
    }
}

void goto_xy(float x, float y){
    float d1 = sqrt((OFFSET1-x)*(OFFSET1-x) + (Y-y)*(Y-y));
    float d2 = sqrt((OFFSET2-x)*(OFFSET2-x) + (Y-y)*(Y-y));
    float angle1, angle2;

    if (x>OFFSET1) {
        angle1 = PI + (isnan(acos(d1/(2*L)))? 0 :acos(d1/(2*L))) - atan((x-OFFSET1)/(Y-y));       //radians
    }
    else {
        angle1 = PI + (isnan(acos(d1/(2*L)))? 0 :acos(d1/(2*L))) + atan((OFFSET1-x)/(Y-y));       //radians
    }

  // ----- calculate motor2 angle when pen at start position (0,0)
    if (x > OFFSET2) {
        angle2 = PI - (isnan(acos(d2/(2*L)))? 0 :acos(d2/(2*L))) - atan((x-OFFSET2)/(Y-y));       //radians
    }
    else {
        angle2 = PI - (isnan(acos(d2/(2*L)))? 0 :acos(d2/(2*L))) + atan((OFFSET2-x)/(Y-y));       //radians
    }
    // printf("PI = %f\n", PI);
    angle1 = angle1 / PI * 180.f - 180.f;
    angle2 = angle2 / PI * 180.f - 180.f;
    //printf("angle1 = %f\n angle2 = %f", angle1, angle2);
    M1.PC1(angle1);
    M2.PC1(angle2);
    //printf("goto (%f, %f), ang1 = %f,  ang2 = %f\n", x, y, angle1, angle2);
    delay_us;
    wait_us(1000000);
}
// void communication(){
//     string mode;
//     int32_t position, speed, direction, ID;
//     position = speed = direction = ID = 0;
//     PC.scanf("%d %s %d %d %d",&ID, &mode[0], &position, &speed, &direction);
//             PC.printf("MANUAL ORDER : MORTOR %d :: %s, %d, %d, %d\n", ID, mode.c_str(), position, speed, direction);

//             if(comp("OL")){
//                 if(ID == 1) M1.OL(position); //power
//                 else M2.OL(position);
//             }
//             else if(comp("SC")){
//                 if(ID == 1) M1.SC(speed);
//                 else M2.SC(speed);
//             }
//             else if(comp("PC1")){
//                 if(ID == 1) M1.PC1(position);
//                 else M2.PC1(position);
//             }
//             else if(comp("PC2")){
//                 if(ID == 1) M1.PC2(position, speed);
//                 else M2.PC2(position, speed);
//             }
//             else if(comp("PC3")){
//                 if(ID == 1) M1.PC3(position, direction);
//                 else M2.PC3(position, direction);
//             }
//             else if(comp("PC4")){
//                 if(ID == 1) M1.PC4(position, speed, direction);
//                 else M2.PC4(position, speed, direction);
//             }
//             while(PC.readable()) PC.getc();
// }