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
| /**
Copyright (c) 2015, Yusuke Doi
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of the FreeBSD Project.
*/
#include <ros.h> // Use ros_lib.
#include <geometry_msgs/Twist.h> // Use Twist.msg. -> [linear(x,y,z), angular(x,y,z)]
/* Define pin numbers. */
#define ACCEL 9
#define BACK_GEAR 8
#define CW 3
#define CCW 4
/* Define pulse width time of the stepping motor. */
#define PULSE_WIDTH_MICRO_SECOND 500
/* Define max speed to 220. */
#define MAX_SPEED 220
/* Define direction of angular z. */
#define LEFT 0
#define RIGHT 1
#define KEEP 2
/* Define direction of back gear. */
#define FORWARD 0
#define BACK 1
/* Declare proto type functions. */
void steer(const char direction); // Write pulse to the stepping motor.
void setGear(const char direction); // Set direction to the back gear pin.
unsigned char getSpeed(float speed); // Get speed what fit the robot model.
void motorCb(const geometry_msgs::Twist& msg); // Call back function of the motor_driver topic.
/* Declare global constants. */
const unsigned int PULSE_FREQUENCY = 1000l * 1000 / (PULSE_WIDTH_MICRO_SECOND * 2); // Get frequency of pulse.
/* Declare global variables. */
ros::NodeHandle nh; // The nodeHandle.
ros::Subscriber<geometry_msgs::Twist> sub("motor_driver", &motorCb); // Set subscribe the motor_driver topic.
void setup() {
/* Set pins Mode. */
for (int i = CCW; i < ACCEL+1; i++) { // 3: CCW, 4: CW, 5: C0, 6:C2, 7:HOFF 8: BACK_GEAR 9: ACCEL
pinMode(i, OUTPUT);
digitalWrite(i, LOW); // Default pin status is LOW.
}
/* Node handle setting. */
nh.initNode(); // First setup the node handle.
nh.subscribe(sub); // Start subscribe the motor_driver topic.
}
void loop() {
nh.spinOnce(); // Check topic and if change it, run the call back function.
delay(1);
}
/**
* Call back function of the motor_driver topic.
* This funcion is moter driver unit.
*
* @author "Yusuke Doi"
* @param msg This param is msg object of the motor_driver topic.
*/
void motorCb(const geometry_msgs::Twist& msg) {
/* gear change task */
if (msg.linear.x < 0 && !digitalRead(BACK_GEAR)) // Go to back, but gear setting is forward now.
setGear(BACK); // Set back to back gear.
else if (msg.linear.x > 0 && digitalRead(BACK_GEAR)) // Go to forward, but gear setting is back now.
setGear(FORWARD); // Set forward to back gear.
/* steer task */
if (msg.angular.z < 0) steer(RIGHT); // Minus mean CCW or right.
else if (msg.angular.z > 0) steer(LEFT); // Plus mean CW or left.
else steer(KEEP); // Zero mean keep steer.
/* move task */
analogWrite(ACCEL, getSpeed(msg.linear.x)); // Write speed to accel pin.
}
/**
* Set back gear by direction.
*
* @author "Yusuke Doi"
* @param direction Set this into back gear.
*/
void setGear(const char direction) {
analogWrite(ACCEL, 0); // Stop the DC motor.
switch (direction) {
case FORWARD:
delay(700); // Wait to stop the DC motor perfectly.
digitalWrite(BACK_GEAR, LOW); // Change back gear to back.
delay(400); // Wait to change back gear perfectly.
break;
case BACK:
delay(500); // Wait to stop the DC motor perfectly.
digitalWrite(BACK_GEAR, HIGH); // Change back gear to back.
delay(500); // Wait to change back gear perfectly.
break;
default:
setGear(FORWARD); // Run this function, and set FORWARD.
}
}
/**
* Steering function of the stepping motor.
*
* @author "Yusuke Doi"
* @param direction Steer to direction.
*/
void steer(const char direction) {
switch (direction) {
case LEFT: // Task steer left.
noTone(CW); // Unset tone. If don't running tone, not happen.
digitalWrite(CW, HIGH); // Set high to CW.
tone(CCW, PULSE_FREQUENCY); // Write pulse to CCW pin. turn to CW.
break;
case RIGHT: // Task steer right.
noTone(CCW); // Unset tone. If don't running tone, not happen.
digitalWrite(CCW, HIGH); // Set high to CCW.
tone(CW, PULSE_FREQUENCY); // Write pulse to CW pin. turn to CCW.
break;
case KEEP: default: // set CW and CCW to low.
noTone(CW); // Unset tone. If don't running tone, not happen.
noTone(CCW); // Unset tone. If don't running tone, not happen.
digitalWrite(CW, LOW);
digitalWrite(CCW, LOW);
}
}
unsigned char getSpeed(float baseSpeed) {
float speed = abs(baseSpeed * MAX_SPEED); // Cast to 0~MAX_SPEED. so baseSpeed width is -1.0~1.0.
if (speed > MAX_SPEED) speed = MAX_SPEED; // Limit max speed to 220.
return (unsigned char)speed; // Cast to unsigned char.
}
|