Ok, this code has been rather hurriedly thrown together and I don't have the hardware set up to test it, so it may need some work, but it should be most of the way there. It should read the joystick on serial port 0 (I would have changed to to port 1 but I'm not sure how), and the topside sends data to the ROV on serial port 2. The ROV's serial port remains unchanged as port 0 however. Easy to change though, if you really want to.
Note I've also updated the code to read heading, pitch, roll, temperature etc from a CMPS-12 module
https://robot-electronics.co.uk/cmps12- ... mpass.html
and display it on a 4x20 display. This all works, but makes the code run a little slow, so I'll probably slim that down for my final code.
Anyway, to the code. Both console & vehicle require updated due to the addition of the compass module.
Console-
Code: Select all
// ///////////////////// //
// //
// ROV Console //
// Modified for PC stick //
// Bennachie 16/09/19 //
// //
// ///////////////////// //
// This code was designed for the fmalpardita library (NewLiquidCrystal)
//This code was designed to suit Scanro's PC Joystick input
#include "joystick.h"
#include <EasyTransfer.h>
#include <Wire.h>
#define baudRate 115200 // Serial port baud rate
#include <LiquidCrystal_I2C.h>
#define I2C_ADDR 0x27 //Address for LCD display. Usually is 0x27 or 0x32.
LiquidCrystal_I2C lcd(I2C_ADDR,2,1,0,4,5,6,7); //Set up library to talk to LCD.
//Create two objects for EasyTransfer.
EasyTransfer ETin, ETout;
struct RECEIVE_DATA_STRUCTURE{
//Put your variable definitions here for the data you want to RECEIVE
//THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
int16_t heading;
int16_t depth;
int16_t battery;
int16_t pitch;
int16_t roll;
int16_t temp;
int16_t align;
};
struct SEND_DATA_STRUCTURE{
//Put your variable definitions here for the data you want to SEND
//THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
int16_t portvert;
int16_t stbdvert;
int16_t portfwd;
int16_t stbdfwd;
int16_t portaft;
int16_t stbdaft;
int16_t lights;
int16_t pan;
int16_t tilt;
int16_t enable;
};
//Give a name to the group of data
RECEIVE_DATA_STRUCTURE rxdata;
SEND_DATA_STRUCTURE txdata;
//Declare global variables
int slider = 511;
int portvalue = 127;
int stbdvalue = 127;
int xaxis = 511;
int yaxis = 511;
int zaxis = 511;
int portfwd1;
int stbdfwd1;
int portaft1;
int stbdaft1;
int16_t portvert;
int16_t stbdvert;
int16_t portfwd;
int16_t stbdfwd;
int16_t portaft;
int16_t stbdaft;
int16_t lights = 0;
int16_t pan = 89;
int16_t tilt = 89;
int lowval = 44;
int midval = 89;
int highval = 179;
float ptspeed = 2;
int enable = 0;
int batt = 0;
int battcon = 0;
int16_t bearing = 0;
signed char pitch = 0;
signed char roll = 0;
void setup(){
//Set the baud rate for the joystick data on serial port 0
Serial.begin(9600);
//Set the baud rate for EasyTransfer data on serial port 2
Serial2.begin(115200);
//start the EasyTransfer library, pass in the data details and the name of the serial port. Can be Serial, Serial1, Serial2, etc.
ETin.begin(details(rxdata), &Serial2);
ETout.begin(details(txdata), &Serial2);
//Set up pins to read buttons on joystick, and thruster enable switch.
pinMode(A4,INPUT_PULLUP);
pinMode(A5,INPUT_PULLUP);
pinMode(A6,INPUT_PULLUP);
pinMode(A7,INPUT_PULLUP);
pinMode(A8,INPUT_PULLUP);
pinMode(A9,INPUT_PULLUP);
pinMode(A10,INPUT_PULLUP);
pinMode(A11,INPUT_PULLUP);
pinMode(A12,INPUT_PULLUP);
pinMode(A13,INPUT_PULLUP);
pinMode(A14,INPUT_PULLUP);
pinMode(12, INPUT_PULLUP);
pinMode(A15, INPUT);
//Set pin for on board indicator LED
pinMode(13, OUTPUT);
//Initialise the LCD
lcd.begin (16, 2);
lcd.setBacklightPin(3,POSITIVE);
//lcd.setBacklight(HIGH); // NOTE: You can turn the backlight off by setting it to LOW instead of HIGH
lcd.setBacklight(LOW);
//Print a splash screen on the LCD
lcd.clear(); // clean screen and sets cursor to (0,0)
Serial.begin(baudRate);
lcd.setBacklight(HIGH); //turn on backlight
lcd.setCursor(0, 0); //First line
lcd.print("********************");
lcd.setCursor(0, 1); //Second line
lcd.print("* Console 16/09/19 *");
lcd.setCursor(20, 0); //Third line
lcd.print("* Version 0.01 *");
lcd.setCursor(20, 1); //Fourth line
lcd.print("********************");
delay(2000);
lcd.clear();
}
void loop(){
//Read values from the joystick axes
//slider = analogRead(A3);
//xaxis = analogRead(A0);
//yaxis = analogRead(A1);
//zaxis = analogRead(A2);
get_joystick();
if(joystick_ok==1)
{
slider = (255-tdata[6]);
portvalue = (((joystick.joystick_x) + (joystick.joystick_y)) - 127);
if (portvalue >= 255){
portvalue = 255;
}
if (portvalue <= 0){
portvalue = 0;
}
stbdvalue = (((joystick.joystick_x) - (joystick.joystick_y)) + 127);
if (stbdvalue >= 255){
stbdvalue = 255;
}
if (stbdvalue <= 0){
stbdvalue = 0;
}
}
//Scale the verts
portvert = (map(slider, 0, 255, 0, 179));
stbdvert = (map(slider, 0, 255, 0, 179));
//send vert values out to EasyTransfer
txdata.portvert = portvert;
txdata.stbdvert = stbdvert;
//Scale thrusters
portaft = map(portvalue, 0, 255, 0, 179);
stbdaft = map(stbdvalue, 0, 255, 0, 179);
//Send thruster values out to EasyTransfer
txdata.portaft = portaft;
txdata.stbdaft = stbdaft;
txdata.portfwd = portfwd;
txdata.stbdfwd = stbdfwd;
//Set pan & tilt values, speed controlled by ptspeed value
if(digitalRead(A4) == LOW) {
pan = pan - ptspeed;
}
if(pan <=0){
pan = 0;
}
if(digitalRead(A5) == LOW) {
pan = pan + ptspeed;
}
if(pan >=179){
pan = 179;
}
if(digitalRead(A6) == LOW) {
tilt = tilt - ptspeed;
}
if(tilt <=0){
tilt = 0;
}
if(digitalRead(A7) == LOW) {
tilt = tilt + ptspeed;
}
if(tilt >=179){
tilt = 179;
}
//Centre P&T if trigger pulled
if(digitalRead(A13) == LOW) {
pan = 89;
tilt = 89;
}
txdata.pan = pan;
txdata.tilt = tilt;
//Send pan & tilt values to EasyTransfer
txdata.pan = pan;
txdata.tilt = tilt;
//Read battery voltage & map voltage to percentage
batt = analogRead(A15);
battcon = map(batt,764, 864, 0, 100);
//Print sensor values to LCD
lcd.clear();
lcd.setCursor(0,0);
lcd.print("H:");
lcd.setCursor(2,0);
lcd.print(rxdata.heading);
lcd.setCursor(7,0);
lcd.print("P:");
lcd.setCursor(9,0);
lcd.print(rxdata.pitch);
lcd.setCursor(14,0);
lcd.print("T:");
lcd.setCursor(16,0);
lcd.print(rxdata.temp);
lcd.setCursor(18,0);
lcd.print((char)223);
lcd.setCursor(19,0);
lcd.print("C");
lcd.setCursor(0,1);
lcd.print("D:");
lcd.setCursor(2,1);
lcd.print(rxdata.depth);
lcd.setCursor(7,1);
lcd.print("R:");
lcd.setCursor(9,1);
lcd.print(rxdata.roll);
lcd.setCursor(14,1);
lcd.print("B:");
lcd.setCursor(16,1);
lcd.print(rxdata.battery);
lcd.setCursor(20,1);
lcd.print("CAL:");
lcd.setCursor(24,1);
lcd.print(rxdata.align);
lcd.setCursor(34,1);
lcd.print("B:");
lcd.setCursor(36,1);
lcd.print(battcon);
enable = digitalRead(12);
txdata.enable = enable;
if(enable == 0) {
lcd.setCursor(20,0);
lcd.print("Disabled");
}
if(enable == 1) {
lcd.setCursor(20,0);
lcd.print("Enabled ");
}
//Read lights pin and send the value to EasyTransfer
if(!digitalRead(A10))
txdata.lights = HIGH;
else
txdata.lights = LOW;
//Read lights pin and set on board LED accordingly
if(!digitalRead(A10))
digitalWrite(13, HIGH);
else
digitalWrite(13, LOW);
//Send all the EasyTransfer data out of the port
ETout.sendData();
//There's a loop here so that we run the receive function more often then the
//transmit function. This is important due to the slight differences in
//the clock speed of different Arduinos. If we didn't do this, messages
//would build up in the buffer and appear to cause a delay.
for(int i=0; i<5; i++){
//remember, you could use an if() here to check for new data, this time it's not needed.
ETin.receiveData();
//delay
delay(10);
}
//delay to slow loop
//delay(10);
}
Vehicle-
Code: Select all
// ///////////////////// //
// //
// ROV Vehicle //
// Bennachie 25/08/19 //
// //
// ///////////////////// //
// This code was designed for the fmalpardita library (NewLiquidCrystal)
#include <Servo.h>
#include <EasyTransfer.h>
#include <Wire.h>
#define baudRate 115200 // Serial port baud rate
#include <LiquidCrystal_I2C.h>
#define I2C_ADDR 0x27 //Address for LCD display. Usually is 0x27 or 0x32.
LiquidCrystal_I2C lcd(I2C_ADDR,2,1,0,4,5,6,7);
//Create two objects for EasyTransfer.
EasyTransfer ETin, ETout;
//create servos
Servo pans;
Servo tilts;
Servo stbdfwds;
Servo stbdverts;
Servo stbdafts;
Servo portfwds;
Servo portverts;
Servo portafts;
struct RECEIVE_DATA_STRUCTURE{
//Put your variable definitions here for the data you want to receive
//THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
int16_t portvert;
int16_t stbdvert;
int16_t portfwd;
int16_t stbdfwd;
int16_t portaft;
int16_t stbdaft;
int16_t lights;
int16_t pan;
int16_t tilt;
int16_t enable;
};
struct SEND_DATA_STRUCTURE{
//Put your variable definitions here for the data you want to send
//THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
int16_t heading;
int16_t depth;
int16_t battery;
int16_t pitch;
int16_t roll;
int16_t temp;
int16_t align;
};
//Give a name to the group of data
RECEIVE_DATA_STRUCTURE rxdata;
SEND_DATA_STRUCTURE txdata;
//Declare global variables
int batt = 0;
//Address of the CMPS12 compass on i2C
#define _i2cAddress 0x60
#define CONTROL_Register 0
#define BEARING_Register 2
#define PITCH_Register 4
#define ROLL_Register 5
#define MAGNET_X_Register 6
#define MAGNET_Y_Register 8
#define MAGNET_Z_Register 10
#define ACCELERO_X_Register 12
#define ACCELERO_Y_Register 14
#define ACCELERO_Z_Register 16
#define _Register_GYRO_X 18
#define _Register_GYRO_Y 20
#define _Register_GYRO_Z 22
#define TEMP_Register 24
#define ALIGN_Register 30
#define COMMUNICATION_TIMEOUT 1000
#define TIMEOUT_MICROSECONDS 1000
#define _ONE_BYTE 1
#define _TWO_BYTES 2
//---------------------------------
int _bearing;
byte _fine;
byte _byteHigh;
byte _byteLow;
char _pitch;
char _roll;
float accelx = 0;
float accely = 0;
float accelz = 0;
float _accelScale = 1.0f/100.f; // 1 m/s^2 = 100 LSB
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
float _gyroScale = 1.0f/16.f; // 1 Dps = 16 LSB
float temp = 0;
int _algn;
//float algn = 0;
void beginI2C(uint8_t _i2cBus, uint32_t _i2cRate);
int16_t heading;
void setup(){
delay (3000);
Serial.begin(115200);
//Start the library, pass in the data details and the name of the serial port. Can be Serial, Serial1, Serial2, etc.
ETin.begin(details(rxdata), &Serial);
ETout.begin(details(txdata), &Serial);
//Set up pins
pinMode(13, OUTPUT);
pinMode(10, OUTPUT);
pinMode(A15, INPUT);
//enable pull-up
//pinMode(12, INPUT_PULLUP);
//Attach servos & thrusters to pins
pans.attach(2);
tilts.attach(3);
portverts.attach(4);
stbdverts.attach(5);
portafts.attach(6);
stbdafts.attach(7);
portfwds.attach(8);
stbdfwds.attach(9);
//Set servos & thrusters to centre (off) position
pans.write(89);
tilts.write(89);
portverts.write(89);
stbdverts.write(89);
portafts.write(89);
stbdafts.write(89);
portfwds.write(89);
stbdfwds.write(89);
//Initialise the LCD
lcd.begin (16, 2);
lcd.setBacklightPin(3,POSITIVE);
//lcd.setBacklight(HIGH); // NOTE: You can turn the backlight off by setting it to LOW instead of HIGH
lcd.setBacklight(LOW);
//Print a splash screen on the LCD
lcd.clear(); // clean screen and sets cursor to (0,0)
Serial.begin(baudRate);
lcd.setBacklight(HIGH); //turn on backlight
lcd.setCursor(0, 0); //First line
lcd.print("ROV 25/08/19");
lcd.setCursor(0, 1); //Second line
lcd.print("Version 1.02");
delay(2000);
lcd.clear();
beginI2C();
}
void loop(){
// read the compass
int bearing = getBearing();
//heading = (int16_t) bearing;
signed char pitch = getPitch();
signed char roll = getRoll();
int temp = getTemp();
int algn = getAlign();
//Read battery voltage
batt = analogRead(A15);
//Map voltage to percentage and send to EasyTransfer
txdata.battery = map(batt,764, 864, 0, 100);
//Placeholders for heading & depth values (to be added)
//txdata.bearing = 0;
txdata.depth = 0;
txdata.heading = bearing;
txdata.pitch = pitch;
txdata.roll = roll;
txdata.temp = temp;
txdata.align = algn;
lcd.clear();
lcd.setCursor(0,0);
lcd.print(bearing);
lcd.setCursor(0,1);
lcd.print(pitch);
lcd.setCursor(8,1);
lcd.print(roll);
lcd.setCursor(5,0);
lcd.print(temp);
lcd.setCursor(10,0);
lcd.print(algn);
//Print input values from Console on to LCD to aid debugging
//lcd.clear();
//lcd.setCursor(0,0);
//lcd.print(rxdata.portfwd);
//lcd.setCursor(0,1);
//lcd.print(rxdata.portaft);
//lcd.setCursor(4,0);
//lcd.print(rxdata.stbdfwd);
//lcd.setCursor(4,1);
//lcd.print(rxdata.stbdaft);
//lcd.setCursor(8,0);
//lcd.print(rxdata.pan);
//lcd.setCursor(8,1);
//lcd.print(rxdata.tilt);
//lcd.setCursor(12,0);
//lcd.print(rxdata.portvert);
//lcd.setCursor(12,1);
//lcd.print(rxdata.lights);
//lcd.setCursor(14,1);
//lcd.print(rxdata.enable);
//Send all the EasyTransfer data out of the port
ETout.sendData();
//There's a loop here so that we run the receive function more often then the
//transmit function. This is important due to the slight differences in
//the clock speed of different Arduinos. If we didn't do this, messages
//would build up in the buffer and appear to cause a delay.
for(int i=0; i<5; i++){
//remember, you could use an if() here to check for new data, this time it's not needed.
ETin.receiveData();
//Turn lights on or off
digitalWrite(13, rxdata.lights);
//This needs work. The lights toggle when given a short input pulse of approx 200ms.
digitalWrite(10, rxdata.lights);
//Send values to servos & thrusters
if(rxdata.enable == 1) {
pans.write(rxdata.pan);
tilts.write(rxdata.tilt);
portverts.write(rxdata.portvert);
stbdverts.write(rxdata.stbdvert);
portafts.write(rxdata.portaft);
stbdafts.write(rxdata.stbdaft);
portfwds.write(rxdata.portfwd);
stbdfwds.write(rxdata.stbdfwd);
}
if(rxdata.enable == 0) {
portverts.write(89);
stbdverts.write(89);
portafts.write(89);
stbdafts.write(89);
portfwds.write(89);
stbdfwds.write(89);
}
//delay
delay(10);
}
//delay to slow loop
//delay(10);
}
void beginI2C() {
Wire.begin();
}
int16_t getBearing()
{
//Setup timeout parameter
int timeout = COMMUNICATION_TIMEOUT;
// begin communication with CMPS11
Wire.beginTransmission(_i2cAddress);
// Tell register you want some data
Wire.write(BEARING_Register);
// End the transmission
int nackCatcher = Wire.endTransmission();
// Return if we have a connection problem
if(nackCatcher != 0){return 0;}
// Request 2 bytes from CMPS11
Wire.requestFrom(_i2cAddress , _TWO_BYTES);
// Wait for the bytes to arrive.
// Don't wait forever as this will hang the whole program
while((Wire.available() < _TWO_BYTES) && (timeout-- >0))
delay(1);
if (timeout <= 0) return -1;
// Read the values
_byteHigh = Wire.read();
_byteLow = Wire.read();
// Calculate full bearing
_bearing = ((_byteHigh<<8) + _byteLow) / 10;
// Calculate bearing decimal
//_fine = ((_byteHigh<<8) + _byteLow) % 10;
return _bearing;
}
byte getPitch()
{
//Setup timeout parameter
int timeout = COMMUNICATION_TIMEOUT;
// begin communication with CMPS11
Wire.beginTransmission(_i2cAddress);
// Tell register you want some data
Wire.write(PITCH_Register);
// End the transmission
int nackCatcher = Wire.endTransmission();
// Return if we have a connection problem
if(nackCatcher != 0){return 0;}
// Request 1 byte from CMPS11
Wire.requestFrom(_i2cAddress , _ONE_BYTE);
// Wait for the bytes to arrive.
// Don't wait forever as this will hang the whole program
while((Wire.available() < _ONE_BYTE) && (timeout-- >0))
delay(1);
if (timeout <= 0) return 0;
// Read the values
_pitch = Wire.read();
return _pitch;
}
byte getRoll()
{
//Setup timeout parameter
int timeout = COMMUNICATION_TIMEOUT;
// begin communication with CMPS11
Wire.beginTransmission(_i2cAddress);
// Tell register you want some data
Wire.write(ROLL_Register);
// End the transmission
int nackCatcher = Wire.endTransmission();
// Return if we have a connection problem
if(nackCatcher != 0){return 0;}
// Request 1 byte from CMPS11
Wire.requestFrom(_i2cAddress , _ONE_BYTE);
// Wait for the bytes to arrive.
// Don't wait forever as this will hang the whole program
while((Wire.available() < _ONE_BYTE) && (timeout-- >0))
delay(1);
if (timeout <= 0) return 0;
// Read the values
_roll = Wire.read();
return _roll ;
}
int16_t getTemp()
{
//Setup timeout parameter
int timeout = COMMUNICATION_TIMEOUT;
// begin communication with CMPS11
Wire.beginTransmission(_i2cAddress);
// Tell register you want some data
Wire.write(TEMP_Register);
// End the transmission
int nackCatcher = Wire.endTransmission();
// Return if we have a connection problem
if(nackCatcher != 0){return 0;}
// Request 2 bytes from CMPS11
Wire.requestFrom(_i2cAddress , _TWO_BYTES);
// Wait for the bytes to arrive.
// Don't wait forever as this will hang the whole program
while((Wire.available() < _TWO_BYTES) && (timeout-- >0))
delay(1);
if (timeout <= 0) return 0;
// Read the values
_byteHigh = Wire.read();
_byteLow = Wire.read();
// Calculate temp
return (((int16_t)_byteHigh <<8) + (int16_t)_byteLow);
}
byte getAlign()
{
//Setup timeout parameter
int timeout = COMMUNICATION_TIMEOUT;
// begin communication with CMPS11
Wire.beginTransmission(_i2cAddress);
// Tell register you want some data
Wire.write(ALIGN_Register);
// End the transmission
int nackCatcher = Wire.endTransmission();
// Return if we have a connection problem
if(nackCatcher != 0){return 0;}
// Request 1 byte from CMPS11
Wire.requestFrom(_i2cAddress , _ONE_BYTE);
// Wait for the bytes to arrive.
// Don't wait forever as this will hang the whole program
while((Wire.available() < _ONE_BYTE) && (timeout-- >0))
delay(1);
if (timeout <= 0) return 0;
// Read the values
_algn = Wire.read();
return _algn;
}