Control

CONTROLLER.CPP

Initialising a thrusterPub publisher that will later publish the /thruster information via the /thruster topic.

This topic is being subscribed by the GUI’s thruster tab where the thruster speeds are displayed.

Subscribing to-

• /imu/HeadingTrue_degree (Published by SpartonCompassIMU)

Corresponding callback- getHeading()- updating the heading_value.

• /pressure (Published by depthtest.py)

Corresponding callback- getPressure()- updating pressure_value.

• /offsetData (Published by frontCam.py containing processed image's estimated gate center coordinates)

Corresponding callback- getOffset()- updating offset.x and y

• /pressurePIDdata and /headingPIDdata

Corresponding callback- initialising PID proportional gain constants. (hKp,hKi,hKd and pKp,pKi,pKd)

Then, initialising thruster speeds-

thruster.speedfwd1 = 1500;
thruster.speedfwd2 = 1500;

thruster.speedup1 = 1500;
thruster.speedup2 = 1500;

thruster.speedup3 = 1500;
thruster.speedup4 = 1500;
thruster.speeddir1 = 1500;
thruster.speeddir2 = 1500;

Heading and pressure controller functions HeadingController(), PressureController() mapping PID constants to PWM values and correspondingly updating thruster direction oriented speeds on the basis of calculated tval.

HeadingController()

thruster.speeddir1 = 1500 - tVal;
thruster.speeddir2 = 1500 - tVal;

PressureController()

thruster.speedup1 = 1500 - tVal;
  thruster.speedup2 = 1500 - tVal;
  thruster.speedup3 = 1500 - tVal;
  thruster.speedup4 = 1500 - tVal;

Finally, publishing the thruster information via the thrusterPub publisher, to be subscribed by the GUI.