Teensy 3.6 + Westward 1

Video

Please click thumbnail image to start the video

Video on Teensy 3.6 + Westward 1 Video on Teensy 3.6 + Westward 1

Photo

Teensy 3.6 + Westward 1
Teensy 3.6 + Westward 1

Sketch

// Teensy 3.6 and Westward Yacht
// Copyright (C) 2019 https://www.roboticboat.uk
// f716cacd-29ff-4a41-a0b7-fb465c6e608d
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <https://www.gnu.org/licenses/>.
// These Terms shall be governed and construed in accordance with the laws of
// England and Wales, without regard to its conflict of law provisions.
// Libraries are available here
// https://github.com/pwesson/roboticboat/tree/master/libraries
#include <SD.h>
#include <Wire.h>
#include <Servo.h>
#include <math.h>
// ____ __ __ ____ ____
// / ___| | \/ | | _ \ / ___|
// | | | |\/| | | |_) | \___ \ .
// | |___ | | | | | __/ ___) |
// \____| |_| |_| |_| |____/
#include <CompassCMPS11.h>
// ____ ____ ____
// / ___| | _ \ / ___|
// | | _ | |_) | \___ \ .
// | |_| | | __/ ___) |
// \____| |_| |____/
#include <AdaUltimateGPS3.h>
// __ __ _ _
// \ \ / / (_) _ __ __| |
// \ \ /\ / / | | | '_ \ / _` |
// \ V V / | | | | | | | (_| |
// \_/\_/ |_| |_| |_| \__,_|
//
#include <Wind.h>
// ____ _ _
// | _ \ _ _ __| | __| | ___ _ __
// | |_) | | | | | / _` | / _` | / _ \ | '__|
// | _ < | |_| | | (_| | | (_| | | __/ | |
// |_| \_\ \__,_| \__,_| \__,_| \___| |_|
#include <Rudder.h>
//Is Robot in control of the human
bool IsHuman = true;
//Radio Control Receiver
int ch1 = 0;
int ch2 = 0;
int ch3 = 0;
//LED light
bool islighton = false;
// Timer
unsigned long mytime;
unsigned long lasttime;
//Radio Control Servo
Servo servoRudder;
Servo servoSail;
int rudder = 95;
// Declare the compass class
CompassCMPS11 compass1;
//A GPS library
AdaUltimateGPS3 gps(&Serial5);
// Global variables
String inputSerial5 = ""; // a string to hold incoming data
boolean IsReadySerial5 = false; // whether the string is complete
// Setup the weighted least squares
float BoatHeading = 0;
float oldBoatHeading = 0;
float desiredHeading = 0;
signed char pitch = 0;
signed char roll = 0;
Wind mywind;
Rudder myrudder;
void setup()
{
// Keep the User informed
Serial.begin(9600);
// initialize the LED pin as an output.
pinMode(13, OUTPUT);
// Setup the input pins used received by the RC receiver
pinMode(6, INPUT);
pinMode(7, INPUT);
pinMode(8, INPUT); //IsRobot flag
// Setup the output pins used by the servos
pinMode(14, OUTPUT);
pinMode(16, OUTPUT);
//Setup servo connection for rudder and sail
servoRudder.attach(14);
servoSail.attach(16);
//Set the rudder range
myrudder.maxposition = 90 + 40;
myrudder.minposition = 90 - 40;
// Initialise the GPS module
Serial.println("Initializing GPS");
Serial5.begin(9600);
delay(100);
// Just want RMC and GGA messages
// Send setup command to the GPS module
Serial5.println("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28");
delay(100);
// 1MHz update
// Send setup command to the GPS module
Serial5.println("$PMTK220,1000*1F");
delay(100);
// Initialize the compass and initial readings
compass1.begin();
BoatHeading = compass1.getBearing();
// Initialise the SD card
if (!SD.begin(BUILTIN_SDCARD))
{
Serial.println("SD failed");
return;
}
//Set the last time
lasttime = 0;
}
void serialEvent5()
{
//This event is called when Serial5 receives new bytes
while (Serial5.available())
{
// Read the new byte:
gps.read((char)Serial5.read());
}
}
void loop()
{
// Timer
mytime = millis();
// Has 1 seconds passed yet?
// Approx once an hour this will roll over to zero again.
if (abs(mytime - lasttime) > 1000)
{
// So we enter this section every 1 second
lasttime = mytime;
//The GPS signal must be active (not void)
if (gps.gpsstatus == 'A')
{
//Flash the light when receiving a GPS signal
if (islighton)
{
// Turn off the light
digitalWrite(13, LOW);
islighton = false;
}
else
{
// Turn on the light
digitalWrite(13, HIGH);
islighton = true;
}
}
else
{
// Turn off the light as no valid GPS location
digitalWrite(13, LOW);
islighton = false;
}
}
// Read the pulse width of each channel.
// The problem here is that the code blocks until it has read the signal.
// This can take up to 20ms (per channel) if the radio control transmitter is off (not transmitting)
ch1 = pulseIn(6, HIGH, 20000);
ch2 = pulseIn(7, HIGH, 20000);
ch3 = pulseIn(8, HIGH, 20000);
// Map the value to the range 0 to 180 which is what radio control servos like
ch1 = map(ch1, 1000, 2000, 0, 180);
ch2 = map(ch2, 1000, 2000, 0, 180);
ch3 = map(ch3, 1000, 2000, 0, 180);
//If the left RC lever is down - turn on robot
if (ch3 > 100)
{
//The transmitter is indicating to turn on the Robot by the position of channel2
if (IsHuman)
{
//The Human was in control, so now initialise the Robot task
//The Robot wants to know the line AB to follow.
myrudder.NewLine(gps.latitude, gps.longitude, BoatHeading);
}
//The Human has handed control of the boat to the Robot
IsHuman = false;
}
else
{
//The Human has regained control of the boat
IsHuman = true;
myrudder.ClearLine();
}
// Read the compass.
compass1.ReadCompass();
BoatHeading = compass1.bearing;
pitch = compass1.pitch;
roll = compass1.roll;
if (BoatHeading == 0) BoatHeading = oldBoatHeading;
// Read the compass gyro
compass1.ReadGyro();
// Read the compass accelerator
compass1.ReadAccelerator();
myrudder.GyroUpdateMean(BoatHeading, compass1.gyroZ);
myrudder.AddGyroReading(rudder, compass1.gyroZ);
//Want to update the Wind direction estimate
mywind.Update(BoatHeading, roll);
mywind.UpdateDistribution(roll);
mywind.UpdateRoll(BoatHeading, roll);
// The Human has to control the boat if there is no GPS signal
if (IsHuman)
{
// Human sets the rudder position
servoRudder.write(ch1);
// Human sets the sails position
servoSail.write(ch2);
//Have sometime for the servo to move
delay(100);
}
else
{
// Update the new rudder position
// Setup a tack - which is estimated wind direction +/- 50 degrees
desiredHeading = myrudder.ChooseTarget(mywind.Direction, BoatHeading);
// Update the rudder to point towards desiredHeading
rudder = myrudder.UpdateTarget(desiredHeading, BoatHeading);
// Robot updates the rudder position
servoRudder.write(rudder);
// Human still controls the sails
servoSail.write(ch2);
}
// Print data to Serial Monitor window
Serial.print("westwood14,$TMR,");
Serial.print(mytime);
Serial.print(",$RC,");
Serial.print(ch1);
Serial.print(",");
Serial.print(ch2);
Serial.print(",");
Serial.print(ch3);
Serial.print("\t$CMP,");
Serial.print(BoatHeading);
Serial.print(",");
Serial.print(pitch);
Serial.print(",");
Serial.print(roll);
Serial.print("\t$GYR,");
Serial.print(compass1.gyroX, 4);
Serial.print(",");
Serial.print(compass1.gyroY, 4);
Serial.print(",");
Serial.print(compass1.gyroZ - myrudder.gyrozmean, 4);
Serial.print(",");
Serial.print(myrudder.gyro.slope);
Serial.print(",");
Serial.print(myrudder.gyro.intercept);
Serial.print("\t$GPS,");
Serial.print(gps.gpsdate);
Serial.print(",");
Serial.print(gps.gpstime);
Serial.print(",");
Serial.print(gps.latitude, 8);
Serial.print(",");
Serial.print(gps.latNS);
Serial.print(",");
Serial.print(gps.longitude, 8);
Serial.print(",");
Serial.print(gps.lonEW);
Serial.print(",");
Serial.print(gps.altitude);
Serial.print(",");
Serial.print(gps.fixquality);
Serial.print(",");
Serial.print(gps.numsatelites);
Serial.print(",");
Serial.print(gps.gpsknots);
Serial.print(",");
Serial.print(gps.gpstrack);
Serial.print(",");
Serial.print(gps.gpsstatus);
Serial.print(",$WND,");
Serial.print(mywind.Direction);
Serial.print(",");
Serial.print(mywind.rollPort);
Serial.print(",");
Serial.print(mywind.rollCentre);
Serial.print(",");
Serial.print(mywind.rollStarboard);
Serial.print("\t$WLS,");
Serial.print(myrudder.wls.slope);
Serial.print(",");
Serial.print(myrudder.wls.intercept);
Serial.print("\t$LINE,");
Serial.print(myrudder.alongtrack);
Serial.print(",");
Serial.print(myrudder.crosstrack);
Serial.print(",$WAYA,");
Serial.print(myrudder.waypointA.latitude, 8);
Serial.print(",");
Serial.print(myrudder.waypointA.longitude, 8);
Serial.print(",$WAYB,");
Serial.print(myrudder.waypointB.latitude, 8);
Serial.print(",");
Serial.print(myrudder.waypointB.longitude, 8);
Serial.print(",$TAR,");
Serial.print(myrudder.targetbearing);
Serial.print(",$RUD,");
Serial.print(rudder);
Serial.print(",");
Serial.print(myrudder.localdistance);
Serial.print(",");
Serial.println(myrudder.sensitivity);
File dataFile = SD.open("datalog.txt", FILE_WRITE);
if (dataFile)
{
dataFile.print("westwood14,$TMR,");
dataFile.print(mytime);
dataFile.print(",$RC,");
dataFile.print(ch1);
dataFile.print(",");
dataFile.print(ch2);
dataFile.print(",");
dataFile.print(ch3);
dataFile.print(",$CMP,");
dataFile.print(BoatHeading);
dataFile.print(",");
dataFile.print(pitch);
dataFile.print(",");
dataFile.print(roll);
dataFile.print(",$ACC,");
dataFile.print(compass1.accelX, 4);
dataFile.print(",");
dataFile.print(compass1.accelY, 4);
dataFile.print(",");
dataFile.print(compass1.accelZ, 4);
dataFile.print(",$GYR,");
dataFile.print(compass1.gyroX, 4);
dataFile.print(",");
dataFile.print(compass1.gyroY, 4);
dataFile.print(",");
dataFile.print(compass1.gyroZ, 4);
dataFile.print(",$GPS,");
dataFile.print(gps.gpsdate, 0);
dataFile.print(",");
dataFile.print(gps.gpstime, 0);
dataFile.print(",");
dataFile.print(gps.latitude, 8);
dataFile.print(",");
dataFile.print(gps.latNS);
dataFile.print(",");
dataFile.print(gps.longitude, 8);
dataFile.print(",");
dataFile.print(gps.lonEW);
dataFile.print(",");
dataFile.print(gps.altitude);
dataFile.print(",");
dataFile.print(gps.fixquality);
dataFile.print(",");
dataFile.print(gps.numsatelites);
dataFile.print(",");
dataFile.print(gps.gpsknots);
dataFile.print(",");
dataFile.print(gps.gpstrack);
dataFile.print(",");
dataFile.print(gps.gpsstatus);
dataFile.print(",$WND,");
dataFile.print(mywind.Direction);
dataFile.print(",");
dataFile.print(mywind.rollPort);
dataFile.print(",");
dataFile.print(mywind.rollCentre);
dataFile.print(",");
dataFile.print(mywind.rollStarboard);
dataFile.print(",$WLS,");
dataFile.print(myrudder.wls.slope);
dataFile.print(",");
dataFile.print(myrudder.wls.intercept);
dataFile.print(",$LINE,");
dataFile.print(myrudder.alongtrack);
dataFile.print(",");
dataFile.print(myrudder.alongtrackobjective);
dataFile.print(",");
dataFile.print(myrudder.crosstrack);
dataFile.print(",$WAYA,");
dataFile.print(myrudder.waypointA.latitude, 8);
dataFile.print(",");
dataFile.print(myrudder.waypointA.longitude, 8);
dataFile.print(",$WAYB,");
dataFile.print(myrudder.waypointB.latitude, 8);
dataFile.print(",");
dataFile.print(myrudder.waypointB.longitude, 8);
dataFile.print(",$WAYD,");
dataFile.print(myrudder.waypointD.latitude, 8);
dataFile.print(",");
dataFile.print(myrudder.waypointD.longitude, 8);
dataFile.print(",$WAYE,");
dataFile.print(myrudder.waypointE.latitude, 8);
dataFile.print(",");
dataFile.print(myrudder.waypointE.longitude, 8);
dataFile.print(",$TAR,");
dataFile.print(myrudder.targetbearing);
dataFile.print(",$RUD,");
dataFile.print(rudder);
dataFile.print(",");
dataFile.print(myrudder.localdistance);
dataFile.print(",");
dataFile.println(myrudder.sensitivity);
dataFile.close();
}
}
view raw boat_Westward_14.ino hosted with ❤ by GitHub
RoboticBoat.uk