-
Notifications
You must be signed in to change notification settings - Fork 0
/
teensy_drive.ino
130 lines (110 loc) · 3.89 KB
/
teensy_drive.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
#include <ros.h> // header files sourced from Step 3
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Empty.h>
#include <race/drive_values.h>
ros::NodeHandle nh;
boolean flagStop = false; // These values were cacluated for the specific Teensy microcontroller using an oscilloscope.
int pwm_center_value = 9830; // 15% duty cycle - corresponds to zero velocity, zero steering
int pwm_lowerlimit = 6554; // 10% duty cycle - corresponds to max reverse velocity, extreme left steering
int pwm_upperlimit = 13108; // 20% duty cycle - corresponds to max forward velocity, extreme right steering
std_msgs::Int32 str_msg; // creater a ROS Publisher called chatter of type str_msg
ros::Publisher chatter("chatter", &str_msg);
int kill_pin = 2; // This is the GPIO pin for emergency stopping.
unsigned long duration = 0;
void messageDrive( const race::drive_values& pwm )
{
// Serial.print("Pwm drive : ");
// Serial.println(pwm.pwm_drive);
// Serial.print("Pwm angle : ");
// Serial.println(pwm.pwm_angle);
if(flagStop == false)
{
str_msg.data = pwm.pwm_drive;
chatter.publish( &str_msg );
if(pwm.pwm_drive < pwm_lowerlimit) // Pin 5 is connected to the ESC..dive motor
{
analogWrite(5,pwm_lowerlimit); // Safety lower limit
}
else if(pwm.pwm_drive > pwm_upperlimit)
{
analogWrite(5,pwm_upperlimit); // Safety upper limit
}
else
{
analogWrite(5,pwm.pwm_drive); // Incoming data
}
if(pwm.pwm_angle < pwm_lowerlimit) // Pin 6 is connected to the steering servo.
{
analogWrite(6,pwm_lowerlimit); // Safety lower limit
}
else if(pwm.pwm_angle > pwm_upperlimit)
{
analogWrite(6,pwm_upperlimit); // Safety upper limit
}
else
{
analogWrite(6,pwm.pwm_angle); // Incoming data
}
}
else
{
analogWrite(5,pwm_center_value);
analogWrite(6,pwm_center_value);
}
}
void messageEmergencyStop( const std_msgs::Bool& flag )
{
flagStop = flag.data;
if(flagStop == true)
{
analogWrite(5,pwm_center_value);
analogWrite(6,pwm_center_value);
}
}
ros::Subscriber<race::drive_values> sub_drive("drive_pwm", &messageDrive ); // Subscribe to drive_pwm topic sent by Jetson
ros::Subscriber<std_msgs::Bool> sub_stop("eStop", &messageEmergencyStop ); // Subscribe to estop topic sent by Jetson
void setup() {
// Need to produce PWM signals so we need to setup the PWM registers. This setup happens next.
analogWriteFrequency(5, 100); // freq at which PWM signals is generated at pin 5.
analogWriteFrequency(6, 100);
analogWriteResolution(16); // Resolution for the PWM signal
analogWrite(5,pwm_center_value); // Setup zero velocity and steering.
analogWrite(6,pwm_center_value);
pinMode(13,OUTPUT); // Teensy's onboard LED pin.
digitalWrite(13,HIGH); // Setup LED.
pinMode(kill_pin,INPUT); // Set emergency pin to accept inputs.
// digitalWrite(2,LOW);
nh.initNode(); // intialize ROS node
nh.subscribe(sub_drive); // start the subscribers.
nh.subscribe(sub_stop);
nh.advertise(chatter); // start the publisher..can be used for debugging.
}
void loop() {
nh.spinOnce();
duration = pulseIn(kill_pin, HIGH, 30000); // continuously monitor the kill pin.
while(duration > 1900) // stop if kill pin activated..setup everything to zero.
{
duration = pulseIn(kill_pin, HIGH, 30000);
analogWrite(5,pwm_center_value);
analogWrite(6,pwm_center_value);
}
// put your main code here, to run repeatedly:
/*
if(Serial.available())
{
int spd = Serial.read();
if(spd>127) {
spd = spd-128;
spd = map(spd,0,100,410,820);
analogWrite(5,spd);
}
else {
//angle servo
spd = map(spd,0,100,410,820);
analogWrite(6,spd);
}
}
*/
}