/
Motors.cpp
103 lines (89 loc) · 2.24 KB
/
Motors.cpp
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
/*
* Motors.cpp
*
* Created on: 25.09.2017
* Author: anonymous
*/
#include <Arduino.h>
#include "defines.h"
#include "globals.h"
// Set speed of Stepper Motor1
// tspeed could be positive or negative (reverse)
void setMotorSpeedM1(int16_t tspeed)
{
long timer_period;
int16_t speed;
// Limit max speed?
// WE LIMIT MAX ACCELERATION of the motors
if ((speed_M1 - tspeed) > MAX_ACCEL)
speed_M1 -= MAX_ACCEL;
else if ((speed_M1 - tspeed) < -MAX_ACCEL)
speed_M1 += MAX_ACCEL;
else
speed_M1 = tspeed;
#if MICROSTEPPING==16
speed = speed_M1 * 50; // Adjust factor from control output speed to real motor speed in steps/second
#else
speed = speed_M1 * 25; // 1/8 Microstepping
#endif
if (speed == 0)
{
timer_period = ZERO_SPEED;
dir_M1 = 0;
}
else if (speed > 0)
{
timer_period = 2000000 / speed; // 2Mhz timer
dir_M1 = 1;
digitalWrite(PIN_MOTOR1_DIR, HIGH);
}
else
{
timer_period = 2000000 / -speed;
dir_M1 = -1;
digitalWrite(PIN_MOTOR1_DIR, LOW);
}
if (timer_period > ZERO_SPEED) // Check for minimun speed (maximun period without overflow)
timer_period = ZERO_SPEED;
timerAlarmWrite(timer1, timer_period, true);
}
// Set speed of Stepper Motor2
// tspeed could be positive or negative (reverse)
void setMotorSpeedM2(int16_t tspeed)
{
long timer_period;
int16_t speed;
// Limit max speed?
// WE LIMIT MAX ACCELERATION of the motors
if ((speed_M2 - tspeed) > MAX_ACCEL)
speed_M2 -= MAX_ACCEL;
else if ((speed_M2 - tspeed) < -MAX_ACCEL)
speed_M2 += MAX_ACCEL;
else
speed_M2 = tspeed;
#if MICROSTEPPING==16
speed = speed_M2 * 50; // Adjust factor from control output speed to real motor speed in steps/second
#else
speed = speed_M2 * 25; // 1/8 Microstepping
#endif
if (speed == 0)
{
timer_period = ZERO_SPEED;
dir_M2 = 0;
}
else if (speed > 0)
{
timer_period = 2000000 / speed; // 2Mhz timer
dir_M2 = 1;
digitalWrite(PIN_MOTOR2_DIR, LOW);
}
else
{
timer_period = 2000000 / -speed;
dir_M2 = -1;
digitalWrite(PIN_MOTOR2_DIR, HIGH);
}
if (timer_period > ZERO_SPEED) // Check for minimun speed (maximun period without overflow)
timer_period = ZERO_SPEED;
timerAlarmWrite(timer2, timer_period, true);
}