Skip to content

Commit 3a39630

Browse files
MSTRoboticsMSTRobotics
committed
Add the arudino file
1 parent 88bb6ac commit 3a39630

File tree

3 files changed

+298
-0
lines changed

3 files changed

+298
-0
lines changed

‎ros_ws/src/arduino/actuator/actuator.ino‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
/**
2+
* @file actuator.ino
3+
* @brief Controls the Dump
4+
*
5+
* Code to be put on the Arduino based Motor Controllers
6+
* Turns Linear Actuators On and Off
7+
*
8+
*/
9+
10+
#include <ros.h>
11+
#include <std_msgs/Int8.h>
12+
13+
const char* DUMP_TOPIC = "dump_activate";
14+
15+
/**
16+
* @brief Pins used to control the Motor
17+
*/
18+
const int FORWARD_PWM_PIN = 10;
19+
const int REVERSE_PWM_PIN = 9;
20+
const int ENABLE_PIN = 7;
21+
22+
void dumpCallback(const std_msgs::Int8& msg);
23+
24+
/**
25+
* @brief ROS node handle
26+
*/
27+
ros::NodeHandle node_handle;
28+
29+
/**
30+
* @brief ROS Subscriber for the Velocity Message
31+
*/
32+
ros::Subscriber<std_msgs::Int8> dumpSub(DUMP_TOPIC, &dumpCallback);
33+
34+
void setup()
35+
{
36+
//setup pins
37+
pinMode(FORWARD_PWM_PIN, OUTPUT);
38+
pinMode(REVERSE_PWM_PIN, OUTPUT);
39+
pinMode(ENABLE_PIN, OUTPUT);
40+
41+
//Set Robot to break when starting
42+
digitalWrite(ENABLE_PIN, HIGH);
43+
analogWrite(FORWARD_PWM_PIN, 0);
44+
analogWrite(REVERSE_PWM_PIN, 0);
45+
46+
//Setup ROS node and topics
47+
node_handle.initNode();
48+
node_handle.subscribe(dumpSub);
49+
}
50+
51+
void loop()
52+
{
53+
//Only used to get the messages from ROS
54+
node_handle.spinOnce();
55+
}
56+
57+
/**
58+
* @brief The callback function for velocity messages
59+
*
60+
* This function accepts velocity messages and convertes them into wheel speeds
61+
* This will convert a float into an intger(0-255)
62+
*
63+
* @param msg The message witch is recived from the velocity publisher
64+
*/
65+
void dumpCallback(const std_msgs::Int8& msg)
66+
{
67+
if(msg.data == 1)
68+
{
69+
//Go Forward
70+
digitalWrite(ENABLE_PIN, HIGH);
71+
analogWrite(FORWARD_PWM_PIN, 255);
72+
analogWrite(REVERSE_PWM_PIN, 0);
73+
}
74+
else if(msg.data == -1)
75+
{
76+
//Go in Reverse
77+
digitalWrite(ENABLE_PIN, HIGH);
78+
analogWrite(FORWARD_PWM_PIN, 0);
79+
analogWrite(REVERSE_PWM_PIN, 255);
80+
}
81+
else
82+
{
83+
//active break
84+
digitalWrite(ENABLE_PIN, HIGH);
85+
analogWrite(FORWARD_PWM_PIN, 0);
86+
analogWrite(REVERSE_PWM_PIN, 0);
87+
}
88+
}
89+

‎ros_ws/src/arduino/conveyer/conveyer.ino‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
/**
2+
* @file conveyor.ino
3+
* @brief Controls the conveyor Belt
4+
*
5+
* Code to be put on the Arduino based Motor Controllers
6+
* Converts Angular Velocity (Rad/Sec) to float (0-255)
7+
*
8+
*/
9+
10+
#include <ros.h>
11+
#include <std_msgs/Float64.h>
12+
13+
const char* conveyor_TOPIC = "conveyor_vel";
14+
15+
/**
16+
* @brief Pins used to control the Motor
17+
*/
18+
const int FORWARD_PWM_PIN = 10;
19+
const int REVERSE_PWM_PIN = 9;
20+
const int ENABLE_PIN = 7;
21+
22+
void conveyorCallback(const std_msgs::Float64& msg);
23+
float fScale( float originalMin, float originalMax, float newBegin,
24+
float newEnd, float inputValue, float curve);
25+
26+
/**
27+
* @brief Float used to scale the Speed to
28+
*/
29+
float desired_speed = 0;
30+
31+
bool flag = false;
32+
33+
/**
34+
* @brief Boolean used to store the desired direction
35+
*
36+
* True is Forward, and False is Backwards
37+
*/
38+
float desired_direction;
39+
40+
/**
41+
* @brief ROS node handle
42+
*/
43+
ros::NodeHandle node_handle;
44+
45+
/**
46+
* @brief ROS Subscriber for the Velocity Message
47+
*/
48+
ros::Subscriber<std_msgs::Float64> conveyorSub(conveyor_TOPIC, &conveyorCallback);
49+
50+
void setup()
51+
{
52+
//setup pins
53+
pinMode(FORWARD_PWM_PIN, OUTPUT);
54+
pinMode(REVERSE_PWM_PIN, OUTPUT);
55+
pinMode(ENABLE_PIN, OUTPUT);
56+
57+
//Set Robot to break when starting
58+
digitalWrite(ENABLE_PIN, HIGH);
59+
analogWrite(FORWARD_PWM_PIN, 0);
60+
analogWrite(REVERSE_PWM_PIN, 0);
61+
62+
//Setup ROS node and topics
63+
node_handle.initNode();
64+
node_handle.subscribe(conveyorSub);
65+
}
66+
67+
void loop()
68+
{
69+
//Only used to get the messages from ROS
70+
node_handle.spinOnce();
71+
}
72+
73+
/**
74+
* @brief The callback function for velocity messages
75+
*
76+
* This function accepts velocity messages and convertes them into wheel speeds
77+
* This will convert a float into an intger(0-255)
78+
*
79+
* @param msg The message witch is recived from the velocity publisher
80+
*/
81+
void conveyorCallback(const std_msgs::Float64& msg)
82+
{
83+
if(!flag && msg.data == 0)
84+
{
85+
flag = true;
86+
}
87+
88+
desired_speed = fScale(0, 1, 0, 255, abs(msg.data), 0);
89+
//If msg.data is positive, the set to TRUE
90+
desired_direction = (msg.data > 0);
91+
92+
if(msg.data > 0 && flag)
93+
{
94+
//Go Forward
95+
digitalWrite(ENABLE_PIN, HIGH);
96+
analogWrite(FORWARD_PWM_PIN, desired_speed);
97+
analogWrite(REVERSE_PWM_PIN, 0);
98+
}
99+
else if(msg.data < 0 && flag)
100+
{
101+
//Go in Reverse
102+
digitalWrite(ENABLE_PIN, HIGH);
103+
analogWrite(FORWARD_PWM_PIN, 0);
104+
analogWrite(REVERSE_PWM_PIN, desired_speed);
105+
}
106+
else
107+
{
108+
//active break
109+
digitalWrite(ENABLE_PIN, HIGH);
110+
analogWrite(FORWARD_PWM_PIN, 0);
111+
analogWrite(REVERSE_PWM_PIN, 0);
112+
}
113+
}
114+
115+
/**
116+
* @brief The Function will Scale the Input to the Expected Output
117+
*
118+
* This function accepts the input value, and input range,
119+
* It will the scale the value to the expected output inside
120+
* the given output range
121+
* This Function will also allow the input to be scaled
122+
* with a curve, It is not onyl linear.
123+
*
124+
* @param originalMin - minimum input expected by function
125+
* originalMax - maximum input expected by function
126+
* newBegin - minimum input returned by the function
127+
* newEnd - maximum input returned by the function
128+
* inputValue - the value you want scaled
129+
* curve - excepts a value -10 through 10, 0 being linear
130+
* scaling the values with a curve
131+
*/
132+
float fScale( float original_min, float original_max, float new_begin,
133+
float new_end, float input_value, float curve)
134+
{
135+
136+
float original_range = 0;
137+
float new_range = 0;
138+
float zero_ref_cur_val = 0;
139+
float normalized_cur_val = 0;
140+
float ranged_value = 0;
141+
boolean inv_flag = 0;
142+
143+
144+
// condition curve parameter
145+
// limit range
146+
if (curve > 10)
147+
{
148+
curve = 10;
149+
}
150+
if (curve < -10)
151+
{
152+
curve = -10;
153+
}
154+
155+
// - invert and scale - this seems more intuitive -
156+
// postive numbers give more weight to high end on output
157+
curve = (curve * -.1) ;
158+
// convert linear scale into lograthimic exponent for other pow function
159+
curve = pow(10, curve);
160+
161+
// Check for out of range inputValues
162+
if (input_value < original_min)
163+
{
164+
input_value = original_min;
165+
}
166+
if (input_value > original_max)
167+
{
168+
input_value = original_max;
169+
}
170+
171+
// Zero Refference the values
172+
original_range = original_max - original_min;
173+
174+
if (new_end > new_begin)
175+
{
176+
new_range = new_end - new_begin;
177+
}
178+
else
179+
{
180+
new_range = new_begin - new_end;
181+
inv_flag = 1;
182+
}
183+
184+
zero_ref_cur_val = input_value - original_min;
185+
186+
// normalize to 0 - 1 float
187+
normalized_cur_val = zero_ref_cur_val / original_range;
188+
189+
// Check for originalMin > originalMax - the math for all other cases
190+
// i.e. negative numbers seems to work out fine
191+
if (original_min > original_max )
192+
{
193+
return 0;
194+
}
195+
196+
if (inv_flag == 0)
197+
{
198+
ranged_value = (pow(normalized_cur_val, curve) * new_range) + new_begin;
199+
}
200+
else // invert the ranges
201+
{
202+
ranged_value = new_begin - (pow(normalized_cur_val, curve) * new_range);
203+
}
204+
205+
return ranged_value;
206+
}
207+

‎ros_ws/src/control/launch/robot.launch‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@
2626
<!-- Arduino Stuff -->
2727
<node name="arduino_right" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM0"/>
2828
<node name="arduino_left" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM1"/>
29+
<node name="arduino_conv" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM3"/>
30+
<node name="arduino_dump" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM4"/>
2931

3032
<!-- Sensing and mapping files -->
3133
<include file="$(find control)/launch/slam.launch"/>

0 commit comments

Comments
 (0)