Skip to content

Commit cfd340d

Browse files
JPGC04JPGC04
committed
new display board code
1 parent f5f96fd commit cfd340d

File tree

1 file changed

+69
-99
lines changed

1 file changed

+69
-99
lines changed

‎Display/src/main.cpp‎

Lines changed: 69 additions & 99 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include <Wire.h>
55
#include "MS5837.h"
66
#include <ros.h>
7+
#include <auv_msgs/ThrusterMicroseconds.h>
78
#include <std_msgs/Float32.h>
89
#include <std_msgs/Float64.h>
910
#include <std_msgs/Int32.h>
@@ -46,14 +47,19 @@ MS5837 sensor;
4647
// Initialize ROS node handle
4748
ros::NodeHandle nh;
4849

50+
// Define global variables for ros handling
51+
// Maintains newest version of thruster pwm values and status
52+
uint16_t microseconds[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
53+
int Sthrusters[8];
54+
4955
// Define global variables for ros handling
5056
// Maintains newest version of all variables
5157
float batt_voltage_1_new = 0;
5258
float batt_voltage_2_new = 0;
5359
int thrusters_new[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
5460
int devices_new[] = { 0, 0, 0, 0, 0, 0, 0 };
5561
String status_new = "I am Douglas!";
56-
float quaternions_new[] = { 0, 0, 0, 0 };
62+
float positions_new[] = { 0, 0, 0, 0 };
5763

5864
// Define global variables for display functions
5965
// Maintains version of variables on screen
@@ -63,7 +69,7 @@ uint16_t batt_colours[] = { WHITE, WHITE };
6369
float thrusters_old[] = { -1, -1, -1, -1, -1, -1, -1, -1 };
6470
float devices_old[] = { -1, -1, -1, -1, -1, -1, -1 };
6571
String status_old = "";
66-
String quaternions_old[] = { "", "", "", "" };
72+
String positions_old[] = { "", "", "", "" };
6773

6874
// Status of batteries
6975
boolean BATT1_EMPTY = false;
@@ -142,14 +148,14 @@ void initMainScreen() {
142148
tft.drawLine(0, HEIGHT / 3 + 40 + 45, WIDTH, HEIGHT / 3 + 40 + 45, BLACK);
143149
tft.drawLine(0, HEIGHT - 41, WIDTH, HEIGHT - 41, BLACK);
144150

145-
// Quaternions section
151+
// Positions section
146152
for (int i = 0; i < 4; i++) {
147153
int x = i * WIDTH / 4;
148154
tft.drawRect(x, HEIGHT - 40, WIDTH / 4, 40, BLACK);
149155
tft.setCursor(x + 2, HEIGHT - 38);
150156
tft.setTextSize(1);
151157
if (i == 0) {
152-
tft.print("W");
158+
tft.print("YAW");
153159
} else if (i == 1) {
154160
tft.print("X");
155161
} else if (i == 2) {
@@ -339,16 +345,16 @@ void status(String status) {
339345
status_old = temp_status;
340346
}
341347

342-
// Function to update quaternions display
343-
void quaternions(float W, float X, float Y, float Z) {
344-
char bufferW[5], bufferX[5], bufferY[5], bufferZ[5];
348+
// Function to update positions display
349+
void positions(float YAW, float X, float Y, float Z) {
350+
char bufferYAW[5], bufferX[5], bufferY[5], bufferZ[5];
345351

346-
if (W == 1.0) {
347-
strcpy(bufferW, "1.0");
352+
if (YAW == 1.0) {
353+
strcpy(bufferYAW, "1.0");
348354
} else {
349-
dtostrf(W, 3, 2, bufferW);
350-
if (bufferW[0] == '0') {
351-
memmove(bufferW, bufferW + 1, strlen(bufferW));
355+
dtostrf(YAW, 3, 2, bufferYAW);
356+
if (bufferYAW[0] == '0') {
357+
memmove(bufferYAW, bufferYAW + 1, strlen(bufferYAW));
352358
}
353359
}
354360

@@ -379,26 +385,43 @@ void quaternions(float W, float X, float Y, float Z) {
379385
}
380386
}
381387

382-
String temp_quaternions[] = { bufferW, bufferX, bufferY, bufferZ };
388+
String temp_positions[] = { bufferYAW, bufferX, bufferY, bufferZ };
383389

384390
for (int i = 0; i < 4; i++) {
385391
int x = i * WIDTH / 4;
386392

387-
if (temp_quaternions[i] != quaternions_old[i]) {
393+
if (temp_positions[i] != positions_old[i]) {
388394
tft.fillRect(x + 1 + 8, HEIGHT - 40 + 1 + 6, WIDTH / 4 - 2 - 8, 38 - 6, WHITE);
389395

390396
tft.setCursor(x + 5, HEIGHT - 33);
391397
tft.setTextColor(BLACK);
392398
tft.setTextSize(4);
393-
tft.println(temp_quaternions[i]);
399+
tft.println(temp_positions[i]);
394400
}
395401
}
396402

397403
for (int i = 0; i < 4; i++) {
398-
quaternions_old[i] = temp_quaternions[i];
404+
positions_old[i] = temp_positions[i];
399405
}
400406
}
401407

408+
// function to update thruster statuses
409+
void thrusterStatus(int Sthrusters[]) {
410+
for (int i = 0; i < 8; i++) {
411+
if (microseconds[i] == 1500) {
412+
Sthrusters[i] = 0;
413+
} else {
414+
Sthrusters[i] = 1;
415+
}
416+
}
417+
}
418+
419+
// Callback function that updates microseconds array with values from ros
420+
void commandCb(const auv_msgs::ThrusterMicroseconds& tc){
421+
memcpy(microseconds, tc.microseconds, 8*sizeof(uint16_t));
422+
thrusterStatus(Sthrusters);
423+
}
424+
402425
// Callback function for battery 1 message
403426
void batt1MessageCallback(const std_msgs::Float32& msg) {
404427
batt_voltage_1_new = msg.data;
@@ -409,45 +432,6 @@ void batt2MessageCallback(const std_msgs::Float32& msg) {
409432
batt_voltage_2_new = msg.data;
410433
}
411434

412-
// Callback function for thruster 1 message
413-
void thruster1MessageCallback(const std_msgs::Int32& msg) {
414-
thrusters_new[0] = msg.data;
415-
}
416-
417-
// Callback function for thruster 2 message
418-
void thruster2MessageCallback(const std_msgs::Int32& msg) {
419-
thrusters_new[1] = msg.data;
420-
}
421-
422-
// Callback function for thruster 3 message
423-
void thruster3MessageCallback(const std_msgs::Int32& msg) {
424-
thrusters_new[2] = msg.data;
425-
}
426-
// Callback function for thruster 4 message
427-
void thruster4MessageCallback(const std_msgs::Int32& msg) {
428-
thrusters_new[3] = msg.data;
429-
}
430-
431-
// Callback function for thruster 5 message
432-
void thruster5MessageCallback(const std_msgs::Int32& msg) {
433-
thrusters_new[4] = msg.data;
434-
}
435-
436-
// Callback function for thruster 6 message
437-
void thruster6MessageCallback(const std_msgs::Int32& msg) {
438-
thrusters_new[5] = msg.data;
439-
}
440-
441-
// Callback function for thruster 7 message
442-
void thruster7MessageCallback(const std_msgs::Int32& msg) {
443-
thrusters_new[6] = msg.data;
444-
}
445-
446-
// Callback function for thruster 8 message
447-
void thruster8MessageCallback(const std_msgs::Int32& msg) {
448-
thrusters_new[7] = msg.data;
449-
}
450-
451435
// Callback function for IMU message
452436
void devicesIMUMessageCallback(const std_msgs::Int32& msg) {
453437
devices_new[0] = msg.data;
@@ -488,43 +472,36 @@ void statusMessageCallback(const std_msgs::String& msg) {
488472
status_new = msg.data;
489473
}
490474

491-
// Callback function for quaternion W message
492-
void quaternionWMessageCallback(const std_msgs::Float32& msg) {
493-
quaternions_new[0] = msg.data;
475+
// Callback function for position W message
476+
void positionYAWMessageCallback(const std_msgs::Float32& msg) {
477+
positions_new[0] = msg.data;
494478
}
495479

496-
// Callback function for quaternion X message
497-
void quaternionXMessageCallback(const std_msgs::Float32& msg) {
498-
quaternions_new[1] = msg.data;
480+
// Callback function for position X message
481+
void positionXMessageCallback(const std_msgs::Float32& msg) {
482+
positions_new[1] = msg.data;
499483
}
500484

501-
// Callback function for quaternion Y message
502-
void quaternionYMessageCallback(const std_msgs::Float32& msg) {
503-
quaternions_new[2] = msg.data;
485+
// Callback function for position Y message
486+
void positionYMessageCallback(const std_msgs::Float32& msg) {
487+
positions_new[2] = msg.data;
504488
}
505489

506-
// Callback function for quaternion Z message
507-
void quaternionZMessageCallback(const std_msgs::Float32& msg) {
508-
quaternions_new[3] = msg.data;
490+
// Callback function for position Z message
491+
void positionZMessageCallback(const std_msgs::Float32& msg) {
492+
positions_new[3] = msg.data;
509493
}
510494

511495
// Define publisher message variable
512496
std_msgs::Float64 depth_msg;
513497

514498
// Define publishers and subscribers
515499
// Publishes depth
516-
// Subscribes to battery voltages, thruster statuses, device statuses, status message, and quaternions
500+
// Subscribes to battery voltages, thruster microseconds, device statuses, status message, and positions
517501
ros::Publisher DEPTH("/sensors/depth/z", &depth_msg);
518-
ros::Subscriber<std_msgs::Float32> BATT1("/batteries/voltage/1", &batt1MessageCallback);
519-
ros::Subscriber<std_msgs::Float32> BATT2("/batteries/voltage/2", &batt2MessageCallback);
520-
ros::Subscriber<std_msgs::Int32> THRUSTER1("/thrusters/status/1", &thruster1MessageCallback);
521-
ros::Subscriber<std_msgs::Int32> THRUSTER2("/thrusters/status/2", &thruster2MessageCallback);
522-
ros::Subscriber<std_msgs::Int32> THRUSTER3("/thrusters/status/3", &thruster3MessageCallback);
523-
ros::Subscriber<std_msgs::Int32> THRUSTER4("/thrusters/status/4", &thruster4MessageCallback);
524-
ros::Subscriber<std_msgs::Int32> THRUSTER5("/thrusters/status/5", &thruster5MessageCallback);
525-
ros::Subscriber<std_msgs::Int32> THRUSTER6("/thrusters/status/6", &thruster6MessageCallback);
526-
ros::Subscriber<std_msgs::Int32> THRUSTER7("/thrusters/status/7", &thruster7MessageCallback);
527-
ros::Subscriber<std_msgs::Int32> THRUSTER8("/thrusters/status/8", &thruster8MessageCallback);
502+
ros::Subscriber<auv_msgs::ThrusterMicroseconds> sub("/propulsion/microseconds", &commandCb);
503+
ros::Subscriber<std_msgs::Float32> BATT1("/display/batteries/voltage/1", &batt1MessageCallback);
504+
ros::Subscriber<std_msgs::Float32> BATT2("/display/batteries/voltage/2", &batt2MessageCallback);
528505
ros::Subscriber<std_msgs::Int32> DEVICEIMU("/sensors/imu/status", &devicesIMUMessageCallback);
529506
ros::Subscriber<std_msgs::Int32> DEVICEDVL("/sensors/dvl/status", &devicesDVLMessageCallback);
530507
ros::Subscriber<std_msgs::Int32> DEVICEPS("/sensors/depth/status", &devicesPSMessageCallback);
@@ -533,10 +510,10 @@ ros::Subscriber<std_msgs::Int32> DEVICEACT("/sensors/actuator/status", &devicesA
533510
ros::Subscriber<std_msgs::Int32> DEVICEFC("/sensors/front_camera/status", &devicesFCMessageCallback);
534511
ros::Subscriber<std_msgs::Int32> DEVICEDC("/sensors/down_camera/status", &devicesDCMessageCallback);
535512
ros::Subscriber<std_msgs::String> STATUSMESSAGE("/mission_display", &statusMessageCallback);
536-
ros::Subscriber<std_msgs::Float32> QUATERNIONW("/quaternions/W", &quaternionWMessageCallback);
537-
ros::Subscriber<std_msgs::Float32> QUATERNIONX("/quaternions/X", &quaternionXMessageCallback);
538-
ros::Subscriber<std_msgs::Float32> QUATERNIONY("/quaternions/Y", &quaternionYMessageCallback);
539-
ros::Subscriber<std_msgs::Float32> QUATERNIONZ("/quaternions/Z", &quaternionZMessageCallback);
513+
ros::Subscriber<std_msgs::Float32> POSITIONYAW("/state/theta/z", &positionYAWMessageCallback);
514+
ros::Subscriber<std_msgs::Float32> POSITIONX("/state/x", &positionXMessageCallback);
515+
ros::Subscriber<std_msgs::Float32> POSITIONY("/state/y", &positionYMessageCallback);
516+
ros::Subscriber<std_msgs::Float32> POSITIONZ("/state/z", &positionZMessageCallback);
540517

541518
// Function to calculate and publish depth
542519
void publish_depth() {
@@ -547,9 +524,9 @@ void publish_depth() {
547524
void setup() {
548525
// Initialize I2C communication with sensor
549526
Wire.begin();
550-
while (!sensor.init()) {
551-
delay(5000);
552-
}
527+
sensor.init();
528+
delay(1000);
529+
553530
sensor.setModel(MS5837::MS5837_30BA);
554531
sensor.setFluidDensity(997);
555532

@@ -568,16 +545,9 @@ void setup() {
568545
nh.initNode();
569546

570547
// Subscribe to ROS topics
548+
nh.subscribe(sub);
571549
nh.subscribe(BATT1);
572550
nh.subscribe(BATT2);
573-
nh.subscribe(THRUSTER1);
574-
nh.subscribe(THRUSTER2);
575-
nh.subscribe(THRUSTER3);
576-
nh.subscribe(THRUSTER4);
577-
nh.subscribe(THRUSTER5);
578-
nh.subscribe(THRUSTER6);
579-
nh.subscribe(THRUSTER7);
580-
nh.subscribe(THRUSTER8);
581551
nh.subscribe(DEVICEIMU);
582552
nh.subscribe(DEVICEDVL);
583553
nh.subscribe(DEVICEPS);
@@ -586,10 +556,10 @@ void setup() {
586556
nh.subscribe(DEVICEFC);
587557
nh.subscribe(DEVICEDC);
588558
nh.subscribe(STATUSMESSAGE);
589-
nh.subscribe(QUATERNIONW);
590-
nh.subscribe(QUATERNIONX);
591-
nh.subscribe(QUATERNIONY);
592-
nh.subscribe(QUATERNIONZ);
559+
//nh.subscribe(POSITIONYAW);
560+
//nh.subscribe(POSITIONX);
561+
//nh.subscribe(POSITIONY);
562+
//nh.subscribe(POSITIONZ);
593563

594564
// Advertise ROS publisher
595565
nh.advertise(DEPTH);
@@ -612,10 +582,10 @@ void loop() {
612582
// Update display with new data
613583
batt1(batt_voltage_1_new);
614584
batt2(batt_voltage_2_new);
615-
thrusters(thrusters_new[0], thrusters_new[1], thrusters_new[2], thrusters_new[3], thrusters_new[4], thrusters_new[5], thrusters_new[6], thrusters_new[7]);
585+
thrusters(Sthrusters[0], Sthrusters[1], Sthrusters[2], Sthrusters[3], Sthrusters[4], Sthrusters[5], Sthrusters[6], Sthrusters[7]);
616586
devices(devices_new[0], devices_new[1], devices_new[2], devices_new[3], devices_new[4], devices_new[5], devices_new[6]);
617587
status(status_new);
618-
quaternions(quaternions_new[0], quaternions_new[1], quaternions_new[2], quaternions_new[3]);
588+
//positions(positions_new[0], positions_new[1], positions_new[2], positions_new[3]);
619589

620590
// Delay for stability
621591
delay(10);

0 commit comments

Comments
 (0)