v-plotter-cr/moveXY.c at master · controlonline/v-plotter-cr · 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
//gcc moveXY.c -lwiringPi -lm -o moveXY.c

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>
#include <math.h>
#include <string.h>

//wiringPI
#define STEP_LEFT 4
#define DIR_LEFT 17
#define STEP_RIGHT 23
#define DIR_RIGHT 22

#define STEP_PAUSE 1500

#define BASELENGTH              845
#define CORDLENGTH_LEFT         220
#define CORDLENGTH_RIGHT        667


int MakeStepLeft(dir)
{
digitalWrite(DIR_LEFT, dir);

digitalWrite(STEP_LEFT, 1);
usleep(STEP_PAUSE);
digitalWrite(STEP_LEFT,0);
usleep(STEP_PAUSE);
printf("left-%d\n",dir);
return 0;
}

int MakeStepRight(dir)
{
digitalWrite(DIR_RIGHT, dir);

digitalWrite(STEP_RIGHT, 1);
usleep(STEP_PAUSE);
digitalWrite(STEP_RIGHT,0);
usleep(STEP_PAUSE);
printf("RIGTH-%d\n",dir);
return 0;
}

int main (int argc, char **argv)
{
wiringPiSetup();
pinMode (STEP_RIGHT, OUTPUT);
pinMode (DIR_RIGHT, OUTPUT);
pinMode (STEP_LEFT, OUTPUT);
pinMode (DIR_LEFT, OUTPUT);

long X =atol(argv[1]);
long Y =atol(argv[2]);

printf("X=%i\nY=%i\n",X,Y);


//Steps per mm (with gear)
//double StepsPermm = 2000.0 / 98.0;== 20,4
//Steps per mm (without gear)
//double StepsPermm = 2000.0 / 199.0;== 10,05
double StepsPermm = (200.0/(8.0*3.141592654)); //7,95
//double StepsPermm = 7.957747155;
long CordLengthLeft = 0;
long CordLengthRight = 0;
long BaseLength = 0;
long X0, Y0;
long newCordLengthLeft;
long newCordLengthRight;

BaseLength = BASELENGTH * StepsPermm;
CordLengthLeft = CORDLENGTH_LEFT * StepsPermm;
CordLengthRight = CORDLENGTH_RIGHT * StepsPermm;

X0 = (CordLengthLeft * CordLengthLeft - CordLengthRight * CordLengthRight + BaseLength * BaseLength) / (2.0 * BaseLength);
Y0 = sqrt(CordLengthRight * CordLengthRight - (BaseLength - X0) * (BaseLength - X0));

printf("X0=%ld, Y0=%ld, CL=%ld, CR=%ld StepsPermm=%lf\n", X0, Y0, CordLengthLeft, CordLengthRight, StepsPermm);

X+=X0;
Y+=Y0;

newCordLengthLeft = sqrt((double)(X * X) + (double)(Y * Y));
newCordLengthRight = sqrt((double)((BaseLength-X) * (BaseLength-X)) + (double)(Y * Y));

  while(newCordLengthLeft > CordLengthLeft){
    printf("MakeStepLeft(1)\n");
    CordLengthLeft++;
  }
  while(newCordLengthLeft < CordLengthLeft){
    printf("MakeStepLeft(-1)\n");
    CordLengthLeft--;
  }

  while(newCordLengthRight > CordLengthRight){
    printf("MakeStepRight(1)\n");
    CordLengthRight++;
  }
  while(newCordLengthRight < CordLengthRight){
    printf("MakeStepRight(-1)\n");
    CordLengthRight--;
  }

return 0;
}