-
Notifications
You must be signed in to change notification settings - Fork 0
/
headMeka_ArduinoROS.ino
190 lines (146 loc) · 4.41 KB
/
headMeka_ArduinoROS.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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
/* Purpose: This sketch headMeka_ArduinoROS uses ROS as well as MultiStepper, AccelStepper libraries to control the
* head of the Meka. It doesn't include the sensors to check
* where is the HOME, but it will be easy to integrate
*
* It gets stucked on the loop, so it would be nice to send little steps
* through a script in ROS
*
* Author: Enrique Ortega
*
* based on Jesse Weisberg's moveo_ros
*
* #if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
*/
#include <ros.h>
#include <head_meka_control/HeadJoint.h>
#include <std_msgs/Int16.h>
// MultiStepper.pde
// -*- mode: C++ -*-
// Use MultiStepper class to manage multiple steppers and make them all move to
// the same position at the same time for linear 2d (or 3d) motion.
#include <AccelStepper.h>
#include <MultiStepper.h>
// EG X-Y position bed driven by 2 steppers
#define ENABLE_ALL 8
// Joint 1
#define E1_STEP_PIN 2
#define E1_DIR_PIN 5
// Joint 2
#define Z_STEP_PIN 4
#define Z_DIR_PIN 7
//#define Z_ENABLE_PIN
//#define Z_MIN_PIN
//#define Z_MAX_PIN
AccelStepper joint1(1,E1_STEP_PIN, E1_DIR_PIN);
AccelStepper joint2(1,Z_STEP_PIN, Z_DIR_PIN);
MultiStepper steppers;
const int fincarrera1 = 9; // the number of the pushbutton pi
const int fincarrera2 = 11;
const int ledPin = 13;
int buttonState1 = 0; // variable for reading the pushbutton status
int buttonState2 = 0;
bool estado;
bool estado2;
int joint_step[2];
int joint_status = 0;
int posicion;
long positions[2]; // Array of desired stepper positions
void head_cb(const head_meka_control::HeadJoint& head_steps){
joint_status = 1;
joint_step[0] = head_steps.position1;
joint_step[1] = head_steps.position2;
}
ros::NodeHandle nh;
//std_msgs::Int16 msg;
head_meka_control::HeadJoint msg;
ros::Publisher steps("joint_steps_feedback",&msg);
//instantiate subscribers
ros::Subscriber<head_meka_control::HeadJoint> head_sub("joint_steps",head_cb); //subscribes to joint_steps on head
void initialjoint1(){
if (buttonState1 != HIGH){
joint1.moveTo(10000);
estado = true;
}
else{
joint1.moveTo(-10000);
estado = false;
}
//digitalWrite(ledPin, buttonState1);
while (buttonState1 != estado){
buttonState1 = digitalRead(fincarrera1);
digitalWrite(ledPin, buttonState1);
joint1.run();
}
joint1.setCurrentPosition(3900); //This will change depending on the angle of the head
if (estado){ //Esto es para que si se resetea por ROS no de problemas
joint1.runToNewPosition(30);
}
else{
joint1.runToNewPosition(-30);
}
}
void initialjoint2(){
if (buttonState2 != HIGH){
joint2.moveTo(-100);
estado2 = true;
}
else{
joint2.moveTo(100);
estado2 = false;
}
//digitalWrite(ledPin, buttonState1);
while (buttonState2 != estado2){
buttonState2 = digitalRead(fincarrera2);
joint2.run();
}
joint2.setCurrentPosition(-40); //This will change depending on the angle of the head
if (estado2){ //Esto es para que si se resetea por ROS no de problemas
joint2.runToNewPosition(5);
}
else{
joint2.runToNewPosition(-5);
}
}
void setup() {
pinMode(13,OUTPUT);
pinMode(ENABLE_ALL, OUTPUT);
pinMode(fincarrera1, INPUT_PULLUP);
pinMode(fincarrera2, INPUT_PULLUP);
digitalWrite(ENABLE_ALL, LOW);
joint_status = 1;
nh.initNode();
nh.subscribe(head_sub);
nh.advertise(steps);
// Configure each stepper
joint1.setMaxSpeed(500);
joint1.setAcceleration(150);
joint2.setMaxSpeed(100);
joint2.setAcceleration(50);
// Then give them to MultiStepper to manage
steppers.addStepper(joint1);
steppers.addStepper(joint2);
initialjoint1();
initialjoint2();
delay(1000);
//digitalWrite(13, 1); //toggle led DEACTIVATED TO AVOID ERRORS
}
void loop() {
if (joint_status == 1) // If command callback (arm_cb) is being called, execute stepper command
{
long positions[5]; // Array of desired stepper positions must be long
positions[0] = joint_step[0]; // negated since the real robot rotates in the opposite direction as ROS
positions[1] = joint_step[1];
// Publish back to ros to check if everything's correct
msg.position1 = positions[0];
msg.position2 = positions[1];
//msg.data=positions[0];
steps.publish(&msg);
steppers.moveTo(positions);
nh.spinOnce();
steppers.runSpeedToPosition(); // Blocks until all are in position
}
}