Day 4: Revving the Servos!

Today we are going to take a big step in building the autonomous vessel for our ongoing project to develop an autonomous vessel, using the Aquabots client. By activating the servo controllers we really make the most defining step in creating a vessel that really do something! But before we begin, it is good to introduce a few terms that we will consistently use from now on, and is also used in the software:

• Thrust: the "gas" we apply to the vessel. Is between (-100% to 100%)
• Heading: the direction to which the vessel OUGHT to go (0-360 degrees)
• Speed: the actual speed the vessel (e.g, measured by the GPS unit), variable
• Bearing: the actual direction that the vessel is taking (0-360 degrees).
• (Rudder) Angle: difference between heading and bearing (variable)

It will be clear that there is a correlation between thrust and speed, and heading and bearing. While there are few means of controlling the speed, the bearing is another matter. The angle is the dynamic control that can be used to determine the direction of the vessel.

With most regular vessels, the angle will correspond with the rudder of the vessel (usually negatively, as the vessel will normally move in the counterdirection of the angle), but as you can see on the photo, my vessel is controlled by two propellors, and the bearing is determined by the mutual differences in thrust. For this reason we will develop a C++ class called 'ServoController" that uses an abstract concept "angle", of which we can derive concrete classes for different kind of vessels.I am using two Absima ECU-1 servo controllers, one for each propellor. They are connected to pins 4 and 5 of the MEGA, using the special Grove cable that can connect to the Grove Shield on one side, and branches to two J style servo connectors at the other side.

The header file of this class looks like this:

class ServoController {
public: ServoController(void);
   virtual bool isReady();
   virtual void setup();
   virtual bool initialise();
   virtual double getHeading();
   virtual double getThrust();
   virtual bool maintenance();
   virtual void stop();
   virtual void toggle();
   /**
    * The ranges of the serve controller are defined as follows:
    * - angle (0-360)
    * - thrust -100% to 100%
    */
   virtual void loop( double angle, double thrust );
   
    //convenience method..keeps the speed the same
    virtual void loop( double angle );
 protected:
   double angle;//The rudder angle (0-360)
   double thrust;//the thrust
   int thrust_offset; //offset for speed motor
   int heading_offset;
   //Compass compass;
   // create servo object to control the heading
   Servo headingServo; 
   // create servo object to control the thrust 
   Servo thrustServo;  
   bool initialised;
   bool ready;
   bool enable;
   /**
    * Converts an angle (0-360) to a form that fits the servo  
    */
   double getCorrectedAngle( double angle );
   //the offset between the bearing and the desired angle.
   // Depends on the servo that is used
 private:
   int clipRange;
};

The corresponding implementation is as follows:

ServoController::ServoController() {};
void ServoController::setup( ) {
 clipRange = SERVO_RANGE;
 angle = 0;
 enable = true;
 //Standard servo range is 0 - 180, with 90 being stop
 heading_offset = 0;
 headingServo.attach( PWM_OUT_HEADING );
 headingServo.write( 90 );
 thrust_offset = 0;
 thrustServo.attach( PWM_OUT_SPEED );
 thrustServo.write( 90 );
}
boolean ServoController::initialise() {
 ready = true;
 return ready;
}
boolean ServoController::maintenance() {
 int counter = 0;
 int br = 0;
 do {
   Serial.print("Moving to: "); Serial.println( br );
   do {
     counter++;
     counter %= 100;
     int speed = -100 + 2 * counter;
     angle = br;
     loop( br, 0);
     delay(100);
   } while ( counter != 0 );
   br -= 10;
   br = ( 360 + br ) % 360;
 } while (true);
}
double ServoController::getHeading( ) {
 return angle;
}
double ServoController::getThrust( ) {
 return thrust;
}
bool ServoController::isReady() {
 return ready;
}
/**
 * Converts an angle (0-360) to a form that fits the servo
 * (0 - 180 ) => 90 - 180
 * (181-360)  => 0 - 90
*/
double ServoController::getCorrectedAngle( double a ) {
 return ( a <= 180 ) ? map( a, 0, 180, 90, 180) : 
    map( a, 180, 360, 0, 90);
}
/**
 * Stop the vessel
*/
void ServoController::stop( ) {
 headingServo.write( 90 ); 
 thrustServo.write( 90 );
 angle = 0;
 thrust = 0;
}
void ServoController::loop( double a, double t) {
 angle = getCorrectedAngle( a );
 thrust = map( t, -100, 100, 0, 180 );
 headingServo.write( angle + heading_offset );
 thrustServo.write( thrust + thrust_offset );
}
void ServoController::loop( double h) {
 loop( h, thrust );
}

This class is based on a 'classical' situation, where a propellor and a rudder is used. However, my vessel overwrites the 'loop' function with the following code:

void DoublePropellor::loop( double angle, double thrust  ) {
 //correct the angle to the thrust 
 //(0-180 => 0-90; 181-360 => -90-0)
 double corr = (( angle <= 180 ) ? angle : angle - 360) / 2;
 //Adjust the thrust to standard servo parameters
 double adjusted = map( thrust, -100, 100, 0, 180);
 double thrustLeft = ( adjusted - corr ) < 90 ? 
    ( adjusted - corr) : ( adjusted - corr );
 if ((thrustLeft > (90 - heading_offset )) && (thrustLeft < 90 ))
   thrustLeft = thrustLeft - heading_offset;
 else if ((thrustLeft < (90 + heading_offset )) && 
    (thrustLeft > 90 ))
   thrustLeft = thrustLeft + heading_offset;
double thrustRight = ( adjusted + corr ) < 90 ? 
    ( adjusted + corr) : ( adjusted + corr );
if ((thrustRight > (90 - thrust_offset )) && (thrustRight < 90 ))
   thrustRight = thrustRight - thrust_offset;
 else if ((thrustRight < (90 + thrust_offset )) && 
    (thrustRight > 90 ))
   
thrustRight = thrustRight + thrust_offset;
 headingServo.write( thrustLeft );
 thrustServo.write( thrustRight );
}

The example code can be found on GitHub. It'll probably need to be adjusted to suit your specific requirements. If you download the code on your Arduino Mega and register your vessel to the Aquabots Client, you should be able to control the servos through the manual control unit:

Check the "Manual" button, and the sliders for heading and thrust should activate your vessel's motors!

with the work we've done so far, we can now control the boat, but it is still very sensitive and would probably not get us very far on an actual lake. so the next step is to make the boat more robust with a digital compass!