2-Axis driver for stepper motors with the FlySky RC

1#ifdef F_CPU
2#undef F_CPU
3#define F_CPU 8000000L
4#endif
5
6#define  MAX_D_IMPL  9000
7#define MAX_A       9000
8#define LED_MRUG    500UL
9
10#define  X_GND_PIN     10
11#define Y_GND_PIN     9
12#define POT_VCC_PIN   3
13#define  LED_PIN       4
14#define PRZYC_PIN     2
15
16#define X_STEP_PIN  13
17#define  X_EN_PIN    12
18#define X_DIR_PIN   11
19#define X_OS        A6
20#define X_OGR_V     A1
21#define X_OGR_A     A2
22
23#define Y_STEP_PIN  6
24#define Y_EN_PIN    7
25#define Y_DIR_PIN   8
26#define Y_OS        A7
27#define Y_OGR_V     A4
28#define  Y_OGR_A     A5
29
30#define POZ_HIST_JOY_ZERO MAX_D_IMPL/40
31
32#define SPAC    300000UL  //5 min
33#include <avr/sleep.h>
34
35uint64_t cMicr, teraz, czas_bater,  czas_led;
36uint64_t XprMicr, Xd_Micr, YprMicr, Yd_Micr;
37int16_t Xv, Xa, Yv,  Ya;
38bool Xp, Yp;
39
40void wakeUpNow()
41  digitalWrite(LED_PIN, HIGH);
42  digitalWrite(POT_VCC_PIN, HIGH);
43}
44
45void setup()
46{
47   pinMode(X_GND_PIN,  OUTPUT);
48   digitalWrite(X_GND_PIN, LOW);
49   pinMode(Y_GND_PIN, OUTPUT);
50   digitalWrite(Y_GND_PIN, LOW);
51   pinMode(POT_VCC_PIN, OUTPUT);
52   digitalWrite(POT_VCC_PIN,  HIGH);
53   pinMode(LED_PIN, OUTPUT);
54   pinMode(PRZYC_PIN, INPUT_PULLUP);
55   
56   pinMode(X_STEP_PIN, OUTPUT);
57   pinMode(X_EN_PIN, OUTPUT);
58   pinMode(X_DIR_PIN,  OUTPUT);
59   digitalWrite(X_STEP_PIN, LOW);
60   digitalWrite(X_EN_PIN, LOW);  
61   digitalWrite(X_DIR_PIN, LOW);
62   pinMode(X_OS, INPUT);
63   pinMode(X_OGR_V,  INPUT);
64   pinMode(X_OGR_A, INPUT);
65   
66   pinMode(Y_STEP_PIN, OUTPUT);
67   pinMode(Y_EN_PIN, OUTPUT);
68   pinMode(Y_DIR_PIN, OUTPUT);
69   digitalWrite(Y_STEP_PIN,  LOW);
70   digitalWrite(Y_EN_PIN, LOW); 
71   digitalWrite(Y_DIR_PIN, LOW);
72   pinMode(Y_OS, INPUT);
73   pinMode(Y_OGR_V, INPUT);
74   pinMode(Y_OGR_A, INPUT);
75}  
76void sleepNow()
77{
78  digitalWrite(LED_PIN, LOW);
79  digitalWrite(POT_VCC_PIN,  LOW);
80  set_sleep_mode(SLEEP_MODE_PWR_DOWN);
81  sleep_enable();
82  attachInterrupt(0,  wakeUpNow, LOW);
83  sleep_mode();
84  sleep_disable();
85  detachInterrupt(0);
86}
87void  loop()
88{
89  cMicr = micros();
90  teraz = millis();
91  // oś X
92  Xv =  map(analogRead(X_OS), 0, 1023, -MAX_D_IMPL, MAX_D_IMPL);
93  if (Xv > POZ_HIST_JOY_ZERO)
94  {
95    Xv = MAX_D_IMPL - Xv + map(analogRead(X_OGR_V), 0, 1023, MAX_D_IMPL,  1);
96  }
97  else if(Xv < 0-POZ_HIST_JOY_ZERO)
98  {
99    digitalWrite(X_DIR_PIN,  HIGH);
100    Xv += MAX_D_IMPL + map(analogRead(X_OGR_V), 0, 1023, MAX_D_IMPL, 1);
101  }
102  else
103  {
104    digitalWrite(X_EN_PIN, HIGH);
105    digitalWrite(X_DIR_PIN,  LOW);
106    digitalWrite(X_STEP_PIN, LOW);
107    Xv=0;
108    Xa = map(analogRead(X_OGR_A),  0, 1023, MAX_A, 0);
109    digitalWrite(LED_PIN, LOW);
110  }
111  if(Xv)
112  {
113    digitalWrite(X_EN_PIN, LOW);
114    Xd_Micr = long(Xv + (Xa?Xa--:0));
115    if(cMicr  - XprMicr > Xd_Micr)
116    {
117      XprMicr = cMicr;
118      Xp = !Xp;
119      digitalWrite(X_STEP_PIN,  Xp);
120      digitalWrite(LED_PIN, Xp);
121      czas_bater = teraz;
122    }
123  }
124  // oś Y
125  Yv= map(analogRead(Y_OS), 0, 1023, -MAX_D_IMPL, MAX_D_IMPL);
126  if (Yv > POZ_HIST_JOY_ZERO)
127  {
128    Yv = MAX_D_IMPL - Yv + map(analogRead(Y_OGR_V),  0, 1023, MAX_D_IMPL, 1);
129  }
130  else if (Yv < 0-POZ_HIST_JOY_ZERO)
131  {
132    digitalWrite(Y_DIR_PIN, HIGH);
133    Yv += MAX_D_IMPL + map(analogRead(Y_OGR_V),  0, 1023, MAX_D_IMPL, 1);
134  }
135  else
136  {
137    digitalWrite(Y_EN_PIN, HIGH);
138    digitalWrite(Y_DIR_PIN, LOW);
139    digitalWrite(Y_STEP_PIN, LOW);
140    Yv=0;
141    Ya = map(analogRead(Y_OGR_A), 0, 1023, 0, MAX_A);
142  }
143  if(Yv)
144  {
145    digitalWrite(Y_EN_PIN, LOW);
146    Yd_Micr = long(Yv + (Ya?Ya--:0));
147    if(cMicr  - YprMicr > Yd_Micr)
148    {
149      YprMicr = cMicr;
150      Yp = !Yp;
151      digitalWrite(Y_STEP_PIN,  Yp);
152      digitalWrite(LED_PIN, Yp);
153      czas_bater = teraz;
154    }
155  }
156  if((teraz - czas_led > LED_MRUG) && !Xv && !Yv)
157  {
158    czas_led  = teraz;
159    digitalWrite(LED_PIN, HIGH);
160    delay(55);
161  }
162  if(teraz  - czas_bater > SPAC) sleepNow();
163}
164