forked from SpaceLegosFan/Payload_22_23
-
Notifications
You must be signed in to change notification settings - Fork 0
/
TROI-Motor.ino
112 lines (101 loc) · 3.16 KB
/
TROI-Motor.ino
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
#include <AccelStepper.h>
// Need to change the pins for Arduino
#define motorInterfaceType 1
#define leadDIR 12 // DIR1 = 26
#define leadSTEP 14 // STEP1 = 25
#define cameraDIR 26 // DIR2 = 13
#define cameraSTEP 25 // STEP2 = 27
#define DEPLOYSTEPS 5700
// Motor Values
float travel_distance = 7.5;
float travel_distance_per_full_step = 0.00125;
float num_deployment_LeadScrew_steps = DEPLOYSTEPS;
AccelStepper LeadScrewStepper(motorInterfaceType, leadSTEP, leadDIR);
AccelStepper CameraStepper(motorInterfaceType, cameraSTEP, cameraDIR);
float cameraAngle = 0.0;
void setup() {
// put your setup code here, to run once:
Serial.begin(38400);
LeadScrewStepper.setMaxSpeed(400); // 800
LeadScrewStepper.setAcceleration(1000);
LeadScrewStepper.setSpeed(400);
CameraStepper.setMaxSpeed(200);
CameraStepper.setAcceleration(1000);
CameraStepper.setSpeed(200);
}
String serialMessage = "";
void loop(){
updateSerialMessage();
if(serialMessage != ""){
checkSerialMessage();
}
delay(2000);
serialMessage = "";
}
void updateSerialMessage(){
while(Serial.available()>0){
int index = Serial.read();
if(index >= 32 && index < 127){
char letter = index;
serialMessage += letter;
}
}
}
void checkSerialMessage() {
serialMessage.toLowerCase();
if (serialMessage != "") {
if (serialMessage == "run motor")
leadScrewRun();
else if (serialMessage == "change direction")
num_deployment_LeadScrew_steps *= -1;
else if (serialMessage == "print steps")
Serial.println(num_deployment_LeadScrew_steps);
else if(serialMessage == "reset steps")
num_deployment_LeadScrew_steps = DEPLOYSTEPS;
else if(serialMessage == "reset motor"){
int temp = num_deployment_LeadScrew_steps;
num_deployment_LeadScrew_steps = -DEPLOYSTEPS;
serialMessage = "run motor";
num_deployment_LeadScrew_steps = temp;
}
else if(serialMessage == "step down"){
int temp = num_deployment_LeadScrew_steps;
num_deployment_LeadScrew_steps = -50;
serialMessage = "run motor";
num_deployment_LeadScrew_steps = temp;
}
else if(serialMessage == "step up"){
int temp = num_deployment_LeadScrew_steps;
num_deployment_LeadScrew_steps = 50;
serialMessage = "run motor";
num_deployment_LeadScrew_steps = temp;
}
else if (serialMessage.indexOf("steps =") != -1){
num_deployment_LeadScrew_steps = serialMessage.substring(serialMessage.indexOf("=") + 2).toInt();
}
else if (serialMessage.indexOf("camera turn") != -1) {
int angle = serialMessage.substring(serialMessage.indexOf("=") + 2).toInt();
spinCameraStepper(angle);
}
else
Serial.println("Not a valid command!");
serialMessage = "";
}
}
void leadScrewRun() {
LeadScrewStepper.move(num_deployment_LeadScrew_steps);
while (LeadScrewStepper.run()) {}
}
void spinCameraStepper(int angle) {
if (cameraAngle + angle > 180)
angle = angle - 360;
else if (cameraAngle + angle < -180)
angle = angle + 360;
Serial.println(angle);
int steps = angle / 1.8;
cameraAngle += steps * 1.8;
Serial.println(steps);
CameraStepper.move(steps);
while (CameraStepper.run()) {
}
}