Skip to content

Commit faa1985

Browse files
JPGC04JPGC04
committed
Refactored power board code
1 parent 77e068a commit faa1985

File tree

3 files changed

+276
-53
lines changed

3 files changed

+276
-53
lines changed

‎pio_workspace/src/power_ros1_main.cpp‎

Lines changed: 0 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,6 @@ SETUP FOR ROS COMMUNICATION WITH THE POWER BOARD
2222
#include <ros.h>
2323
#include <auv_msgs/ThrusterMicroseconds.h>
2424
#include <std_msgs/Float32.h>
25-
#include <std_msgs/Float32MultiArray.h>
2625

2726
// defines thruster pins
2827
#define BACK_L_PIN 2
@@ -71,18 +70,13 @@ std_msgs::Float32 thruster8_current_msg;
7170
std_msgs::Float32 board_temperature_msg;
7271
std_msgs::Float32 teensy_temperature_msg;
7372

74-
std_msgs::Float32MultiArray all_data_msg;
75-
float allData[21];
76-
7773
// creates array of 8 thrusters
7874
Servo thrusters[8];
7975

8076
// signals to push to thrusters
8177
uint16_t microseconds[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
8278
const uint16_t offCommand[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
8379

84-
float thrusterForce = -1.0;
85-
8680
float Bvoltages[2]; // array for 2 battery voltage sensing
8781
float Tcurrents[8]; // array for 8 thrusters current sensing
8882
float boardTemperature;
@@ -105,10 +99,6 @@ void commandCb(const auv_msgs::ThrusterMicroseconds &tc) {
10599
memcpy(microseconds, tc.microseconds, 8 * sizeof(uint16_t));
106100
}
107101

108-
void thruster_force_Cb(const std_msgs::Float32 &tc) {
109-
thrusterForce = tc.data;
110-
}
111-
112102
// attaches and arms thrusters
113103
void initThrusters() {
114104
thrusters[BACK_L].attach(BACK_L_PIN);
@@ -139,9 +129,6 @@ ros::Publisher thruster8_current("/power/thrusters/current/8", &thruster8_curren
139129
ros::Publisher board_temperature("/power/board/temperature", &board_temperature_msg);
140130
ros::Publisher teensy_temperature("/power/teensy/temperature", &teensy_temperature_msg);
141131

142-
ros::Subscriber<std_msgs::Float32> thrusterforcesub("/thruster/force", &thruster_force_Cb);
143-
ros::Publisher alldatapub("/all/data", &all_data_msg);
144-
145132
// senses the battery voltages, thruster currents, and board and teensy temperatures
146133
void senseData() {
147134
float* voltagePtr = adcSensors.senseVoltage();
@@ -195,39 +182,6 @@ void publishData() {
195182
teensy_temperature.publish(&teensy_temperature_msg);
196183
}
197184

198-
void allDataFunction() {
199-
allData[0] = microseconds[0];
200-
allData[1] = microseconds[1];
201-
allData[2] = microseconds[2];
202-
allData[3] = microseconds[3];
203-
allData[4] = microseconds[4];
204-
allData[5] = microseconds[5];
205-
allData[6] = microseconds[6];
206-
allData[7] = microseconds[7];
207-
208-
allData[8] = thrusterForce;
209-
210-
allData[9] = Tcurrents[0];
211-
allData[10] = Tcurrents[1];
212-
allData[11] = Tcurrents[2];
213-
allData[12] = Tcurrents[3];
214-
allData[13] = Tcurrents[4];
215-
allData[14] = Tcurrents[5];
216-
allData[15] = Tcurrents[6];
217-
allData[16] = Tcurrents[7];
218-
219-
allData[17] = Bvoltages[0];
220-
allData[18] = Bvoltages[1];
221-
222-
allData[19] = boardTemperature;
223-
224-
allData[20] = teensyTemperature;
225-
226-
all_data_msg.data = allData;
227-
228-
alldatapub.publish(&all_data_msg);
229-
}
230-
231185
// setup all thrusters and sensors and setup node handler for subscribing and advertising
232186
void power_ros1_setup() {
233187
pinMode(LED_PIN, OUTPUT);
@@ -252,20 +206,13 @@ void power_ros1_setup() {
252206
nh.advertise(thruster8_current);
253207
nh.advertise(board_temperature);
254208
nh.advertise(teensy_temperature);
255-
nh.subscribe(thrusterforcesub);
256-
257-
all_data_msg.data_length = 21;
258-
259-
nh.advertise(alldatapub);
260209
}
261210

262211
void power_ros1_loop() {
263212
updateThrusters(microseconds);
264213

265214
publishData();
266215

267-
allDataFunction();
268-
269216
nh.spinOnce();
270217

271218
delay(10);

‎pio_workspace/test/thruster_tests/README.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
In order to run the thruster tests, flash the pwoer board with the updated power_ros1_main.cpp file.
2+
This file publishes all data including load cell and power board data to a single rostopic.

‎pio_workspace/test/thruster_tests/power_ros1_main.cpp‎

Lines changed: 274 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,274 @@
1+
/*
2+
Revision 1.0
3+
4+
SETUP FOR ROS COMMUNICATION WITH THE POWER BOARD
5+
1) Initializes and connects thrusters, adc sensors, and temperature sensors
6+
2) Sets up ros nodes for advertising and subscribing
7+
3) Polls all sensors, publishes data, and updates thrusters every 10 ms
8+
*/
9+
10+
#ifdef POWER_ROS1_H
11+
12+
#include "power_ros1_main.h"
13+
14+
#include <Arduino.h>
15+
16+
#include "ThrusterControl.h"
17+
#include "adc_sensors.h"
18+
#include "TMP36.h"
19+
20+
#include <Servo.h>
21+
22+
#include <ros.h>
23+
#include <auv_msgs/ThrusterMicroseconds.h>
24+
#include <std_msgs/Float32.h>
25+
#include <std_msgs/Float32MultiArray.h>
26+
27+
// defines thruster pins
28+
#define BACK_L_PIN 2
29+
#define HEAVE_BACK_L_PIN 3
30+
#define HEAVE_FRONT_L_PIN 4
31+
#define FRONT_L_PIN 5
32+
#define FRONT_R_PIN 6
33+
#define HEAVE_FRONT_R_PIN 7
34+
#define HEAVE_BACK_R_PIN 8
35+
#define BACK_R_PIN 9
36+
37+
#define LED_PIN 13
38+
39+
#define ENABLE_VOLTAGE_SENSE true
40+
#define ENABLE_CURRENT_SENSE true
41+
42+
// creates ADCSensors and TMP36 sensor objects
43+
ADCSensors adcSensors;
44+
TMP36 temperatureSensor(23, 3.3);
45+
46+
// defines 8 thrusters for ROS subscribing
47+
const uint8_t BACK_L = auv_msgs::ThrusterMicroseconds::BACK_LEFT;
48+
const uint8_t HEAVE_BACK_L = auv_msgs::ThrusterMicroseconds::HEAVE_BACK_LEFT;
49+
const uint8_t HEAVE_FRONT_L = auv_msgs::ThrusterMicroseconds::HEAVE_FRONT_LEFT;
50+
const uint8_t FRONT_L = auv_msgs::ThrusterMicroseconds::FRONT_LEFT;
51+
const uint8_t FRONT_R = auv_msgs::ThrusterMicroseconds::FRONT_RIGHT;
52+
const uint8_t HEAVE_FRONT_R = auv_msgs::ThrusterMicroseconds::HEAVE_FRONT_RIGHT;
53+
const uint8_t HEAVE_BACK_R = auv_msgs::ThrusterMicroseconds::HEAVE_BACK_RIGHT;
54+
const uint8_t BACK_R = auv_msgs::ThrusterMicroseconds::BACK_RIGHT;
55+
56+
// defines 2 battery voltage sensing for ROS advertising
57+
std_msgs::Float32 batt1_voltage_msg;
58+
std_msgs::Float32 batt2_voltage_msg;
59+
60+
// defines 8 thruster current sensing for ROS advertising
61+
std_msgs::Float32 thruster1_current_msg;
62+
std_msgs::Float32 thruster2_current_msg;
63+
std_msgs::Float32 thruster3_current_msg;
64+
std_msgs::Float32 thruster4_current_msg;
65+
std_msgs::Float32 thruster5_current_msg;
66+
std_msgs::Float32 thruster6_current_msg;
67+
std_msgs::Float32 thruster7_current_msg;
68+
std_msgs::Float32 thruster8_current_msg;
69+
70+
// defines board and teensy temperature sensing for ROS advertising
71+
std_msgs::Float32 board_temperature_msg;
72+
std_msgs::Float32 teensy_temperature_msg;
73+
74+
std_msgs::Float32MultiArray all_data_msg;
75+
float allData[21];
76+
77+
// creates array of 8 thrusters
78+
Servo thrusters[8];
79+
80+
// signals to push to thrusters
81+
uint16_t microseconds[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
82+
const uint16_t offCommand[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
83+
84+
float thrusterForce = -1.0;
85+
86+
float Bvoltages[2]; // array for 2 battery voltage sensing
87+
float Tcurrents[8]; // array for 8 thrusters current sensing
88+
float boardTemperature;
89+
float teensyTemperature;
90+
91+
// updates thrusters' pwm signals from array
92+
void updateThrusters(const uint16_t microseconds[8]) {
93+
thrusters[BACK_L].writeMicroseconds(microseconds[BACK_L]);
94+
thrusters[HEAVE_BACK_L].writeMicroseconds(microseconds[HEAVE_BACK_L]);
95+
thrusters[HEAVE_FRONT_L].writeMicroseconds(microseconds[HEAVE_FRONT_L]);
96+
thrusters[FRONT_L].writeMicroseconds(microseconds[FRONT_L]);
97+
thrusters[FRONT_R].writeMicroseconds(microseconds[FRONT_R]);
98+
thrusters[HEAVE_FRONT_R].writeMicroseconds(microseconds[HEAVE_FRONT_R]);
99+
thrusters[BACK_R].writeMicroseconds(microseconds[BACK_R]);
100+
thrusters[HEAVE_BACK_R].writeMicroseconds(microseconds[HEAVE_BACK_R]);
101+
}
102+
103+
// updates microseconds array with values from ros
104+
void commandCb(const auv_msgs::ThrusterMicroseconds &tc) {
105+
memcpy(microseconds, tc.microseconds, 8 * sizeof(uint16_t));
106+
}
107+
108+
void thruster_force_Cb(const std_msgs::Float32 &tc) {
109+
thrusterForce = tc.data;
110+
}
111+
112+
// attaches and arms thrusters
113+
void initThrusters() {
114+
thrusters[BACK_L].attach(BACK_L_PIN);
115+
thrusters[HEAVE_BACK_L].attach(HEAVE_BACK_L_PIN);
116+
thrusters[HEAVE_FRONT_L].attach(HEAVE_FRONT_L_PIN);
117+
thrusters[FRONT_L].attach(FRONT_L_PIN);
118+
thrusters[FRONT_R].attach(FRONT_R_PIN);
119+
thrusters[HEAVE_FRONT_R].attach(HEAVE_FRONT_R_PIN);
120+
thrusters[HEAVE_BACK_R].attach(HEAVE_BACK_R_PIN);
121+
thrusters[BACK_R].attach(BACK_R_PIN);
122+
123+
updateThrusters(offCommand);
124+
}
125+
126+
// sets up ros publisher and subscriber nodes
127+
ros::NodeHandle nh;
128+
ros::Subscriber<auv_msgs::ThrusterMicroseconds> sub("/propulsion/microseconds", &commandCb);
129+
ros::Publisher batt1_voltage("/power/batteries/voltage/1", &batt1_voltage_msg);
130+
ros::Publisher batt2_voltage("/power/batteries/voltage/2", &batt2_voltage_msg);
131+
ros::Publisher thruster1_current("/power/thrusters/current/1", &thruster1_current_msg);
132+
ros::Publisher thruster2_current("/power/thrusters/current/2", &thruster2_current_msg);
133+
ros::Publisher thruster3_current("/power/thrusters/current/3", &thruster3_current_msg);
134+
ros::Publisher thruster4_current("/power/thrusters/current/4", &thruster4_current_msg);
135+
ros::Publisher thruster5_current("/power/thrusters/current/5", &thruster5_current_msg);
136+
ros::Publisher thruster6_current("/power/thrusters/current/6", &thruster6_current_msg);
137+
ros::Publisher thruster7_current("/power/thrusters/current/7", &thruster7_current_msg);
138+
ros::Publisher thruster8_current("/power/thrusters/current/8", &thruster8_current_msg);
139+
ros::Publisher board_temperature("/power/board/temperature", &board_temperature_msg);
140+
ros::Publisher teensy_temperature("/power/teensy/temperature", &teensy_temperature_msg);
141+
142+
ros::Subscriber<std_msgs::Float32> thrusterforcesub("/thruster/force", &thruster_force_Cb);
143+
ros::Publisher alldatapub("/all/data", &all_data_msg);
144+
145+
// senses the battery voltages, thruster currents, and board and teensy temperatures
146+
void senseData() {
147+
float* voltagePtr = adcSensors.senseVoltage();
148+
float* currentPtr = adcSensors.senseCurrent();
149+
150+
// Copy the values into the fixed-size arrays
151+
for (int i = 0; i < 2; ++i) {
152+
Bvoltages[i] = voltagePtr[i];
153+
}
154+
155+
for (int i = 0; i < 8; ++i) {
156+
Tcurrents[i] = currentPtr[i];
157+
}
158+
159+
boardTemperature = temperatureSensor.readTemperature();
160+
teensyTemperature = tempmonGetTemp();
161+
}
162+
163+
// updates values sensed onto the ros nodes and publishes them
164+
void publishData() {
165+
senseData();
166+
167+
batt1_voltage_msg.data = Bvoltages[0];
168+
batt2_voltage_msg.data = Bvoltages[1];
169+
170+
batt1_voltage.publish(&batt1_voltage_msg);
171+
batt2_voltage.publish(&batt2_voltage_msg);
172+
173+
thruster1_current_msg.data = Tcurrents[0];
174+
thruster2_current_msg.data = Tcurrents[1];
175+
thruster3_current_msg.data = Tcurrents[2];
176+
thruster4_current_msg.data = Tcurrents[3];
177+
thruster5_current_msg.data = Tcurrents[4];
178+
thruster6_current_msg.data = Tcurrents[5];
179+
thruster7_current_msg.data = Tcurrents[6];
180+
thruster8_current_msg.data = Tcurrents[7];
181+
182+
thruster1_current.publish(&thruster1_current_msg);
183+
thruster2_current.publish(&thruster2_current_msg);
184+
thruster3_current.publish(&thruster3_current_msg);
185+
thruster4_current.publish(&thruster4_current_msg);
186+
thruster5_current.publish(&thruster5_current_msg);
187+
thruster6_current.publish(&thruster6_current_msg);
188+
thruster7_current.publish(&thruster7_current_msg);
189+
thruster8_current.publish(&thruster8_current_msg);
190+
191+
board_temperature_msg.data = boardTemperature;
192+
teensy_temperature_msg.data = teensyTemperature;
193+
194+
board_temperature.publish(&board_temperature_msg);
195+
teensy_temperature.publish(&teensy_temperature_msg);
196+
}
197+
198+
void allDataFunction() {
199+
allData[0] = microseconds[0];
200+
allData[1] = microseconds[1];
201+
allData[2] = microseconds[2];
202+
allData[3] = microseconds[3];
203+
allData[4] = microseconds[4];
204+
allData[5] = microseconds[5];
205+
allData[6] = microseconds[6];
206+
allData[7] = microseconds[7];
207+
208+
allData[8] = thrusterForce;
209+
210+
allData[9] = Tcurrents[0];
211+
allData[10] = Tcurrents[1];
212+
allData[11] = Tcurrents[2];
213+
allData[12] = Tcurrents[3];
214+
allData[13] = Tcurrents[4];
215+
allData[14] = Tcurrents[5];
216+
allData[15] = Tcurrents[6];
217+
allData[16] = Tcurrents[7];
218+
219+
allData[17] = Bvoltages[0];
220+
allData[18] = Bvoltages[1];
221+
222+
allData[19] = boardTemperature;
223+
224+
allData[20] = teensyTemperature;
225+
226+
all_data_msg.data = allData;
227+
228+
alldatapub.publish(&all_data_msg);
229+
}
230+
231+
// setup all thrusters and sensors and setup node handler for subscribing and advertising
232+
void power_ros1_setup() {
233+
pinMode(LED_PIN, OUTPUT);
234+
digitalWrite(LED_PIN, HIGH); // turn on LED_PIN
235+
236+
initThrusters();
237+
238+
adcSensors.begin(ENABLE_VOLTAGE_SENSE, ENABLE_CURRENT_SENSE, &Wire1);
239+
temperatureSensor.begin();
240+
241+
nh.initNode();
242+
nh.subscribe(sub);
243+
nh.advertise(batt1_voltage);
244+
nh.advertise(batt2_voltage);
245+
nh.advertise(thruster1_current);
246+
nh.advertise(thruster2_current);
247+
nh.advertise(thruster3_current);
248+
nh.advertise(thruster4_current);
249+
nh.advertise(thruster5_current);
250+
nh.advertise(thruster6_current);
251+
nh.advertise(thruster7_current);
252+
nh.advertise(thruster8_current);
253+
nh.advertise(board_temperature);
254+
nh.advertise(teensy_temperature);
255+
nh.subscribe(thrusterforcesub);
256+
257+
all_data_msg.data_length = 21;
258+
259+
nh.advertise(alldatapub);
260+
}
261+
262+
void power_ros1_loop() {
263+
updateThrusters(microseconds);
264+
265+
publishData();
266+
267+
allDataFunction();
268+
269+
nh.spinOnce();
270+
271+
delay(10);
272+
}
273+
274+
#endif

0 commit comments

Comments
 (0)