Sender:
#include <Servo.h>
#include <LiquidCrystal.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <stdlib.h>
LiquidCrystal_I2C lcd1(0x27,16,2);
LiquidCrystal_I2C lcd2(0x21,16,2);
const int joy1yPin = 0;
const int joy1xPin = 1;
const int joy2yPin = 2;
const int led = 13;
const int button1 = 2;
const int button2 = 9;
int SS1 = LOW;
int SS2 = LOW;
int start = 0;
int end = 0;
int BB = 0;
int SB = 0;
int Rear = 0;
int RearT =0;
int Dive = 0;
int Light = 0;
int Depth = 0;
int Compass = 0;
int BBTmp = 0;
int SBTmp = 0;
int RearTmp = 0;
int DiveTmp = 0;
int LightTmp = 0;
int LCDBB = 0;
int LCDSB = 0;
int LCDRear = 0;
void setup()
{
pinMode(A0,INPUT);
pinMode(A1,INPUT);
pinMode(A2,INPUT);
pinMode(led,OUTPUT);
pinMode(button1,INPUT);
pinMode(button2,INPUT);
Serial.begin(9600);
lcd1.begin();
lcd1.print("UUV v2.0");
lcd1.setCursor(0,1);
lcd1.print("INITIALIZING...");
lcd2.begin();
lcd2.print("UUV v2.0");
lcd2.setCursor(0,1);
lcd2.print("INITIALIZING...");
delay(1000);
}
void loop(){
Calculations();
Signalsend();
Signalrecieve();
Displays();
}
void Calculations(){
//
// FORWARD REVERSE
//
int joy1y = analogRead(joy1yPin); // variable joystick 1 y value
if (joy1y < 511){ // joystick down
BB = map(joy1y, 0, 511, 10, 90); // scale forward signal to BB ESC/SERVO with
SB = map(joy1y, 0, 511, 10, 90); // scale forward signal to SB ESC/SERVO with
}
else if (joy1y > 515){ // joystick up
BB = map(joy1y, 1023, 511, 170, 90); // scale reverse signal to BB ESC/SERVO with
SB = map(joy1y, 1023, 511, 170, 90); // scale reverse signal to SB ESC/SERVO with
}
else if (joy1y < 515 && joy1y > 511){
BB = 90; // no motor movement
SB = 90; // no motor movement
}
//
// LEFT RIGHT
//
int joy1x = analogRead(joy1xPin); // variable joystick 1 x value
if (joy1x < 511) { // joystick left
int xMapped=map(joy1x,0,511,90,0); // map movement
BB = BB - xMapped; // slow down BB with mapped value
SB = SB + xMapped; // speed up SB with mapped value
if (BB < 10) { // set max speed limit reverse
BB = 10;
}
if (SB > 170) { // set max speed limit forward
SB = 170;
}
}
if (joy1x > 511) { // joystick right
int xMapped=map(joy1x,1023,511,90,0); // map movement with safety limit
BB = BB + xMapped; // speed up BB with mapped value
SB = SB - xMapped; // slow down SB with mapped value
if (SB < 10) { // set max speed limit reverse
SB = 10;
}
if (BB > 170) { // set max speed limit forward
BB = 170;
}
}
//
// DEPTH CONTROL
//
// READ BUTTON
if(digitalRead(button1)== HIGH){
SS1 = 1;
digitalWrite(led, LOW);
}
if(digitalRead(button1)==LOW){
SS1 = 0;
digitalWrite(led, HIGH);
}
// READ JOY
int joy2y = analogRead(joy2yPin);
if (joy2y< 511){ // joystick down
RearT = map(joy2y, 0, 511, 10, 90); // scale reverse signal to depth ESC/SERVO
}
else if (joy2y > 511){ // joystick up
RearT = map(joy2y, 1023, 511, 170, 90); // scale reverse signal to depth ESC/SERVO
}
if (RearT < 10) { // set speed limit dive
RearT = 10;
}
if (RearT > 170) { // set speed limit surface
RearT = 170;
}
if (SS1 == 1){
Rear = RearT;
Dive = 90;
}
else{
Dive = RearT;
Rear = 90;
}
//
// LIGHT
//
//
// if(digitalRead(button2)== HIGH){
// Light = 1;
// digitalWrite(led, LOW);
// }
// if(digitalRead(button2)==LOW){
// Light = 0;
// digitalWrite(led, HIGH);
// }
}
void Signalsend(){
//
// SENDING SIGNALS
//
if(BB != BBTmp || SB != SBTmp || Rear != RearTmp || Dive != DiveTmp || Light != LightTmp)
{
BBTmp = BB;
SBTmp = SB;
RearTmp = Rear;
DiveTmp = Dive;
LightTmp = Light;
Serial.write(9);
Serial.write(BB);
Serial.write(SB);
Serial.write(Rear);
Serial.write(Dive);
Serial.write(Light);
Serial.write(9);
Serial.flush();
}
}
void Signalrecieve(){
//
// RECIEVE SIGNALS
//
//
// start = Serial.read();
//
// Serial.println("Start: ");
// Serial.print(start, DEC);
// if(start == 8){
// int Depth = Serial.read();
// int Compass = Serial.read();
//
// end = Serial.read();
// Serial.println("End: ");
// Serial.print(end, DEC);
// if(end == 8)
// {
//
// lcd2.setCursor(9,0);
// lcd2.print("Compass");
// lcd2.setCursor(9,1);
// lcd2.print("Depth");
//
// }
// }
}
void Displays(){
//
// WRITE DISPLAY 1
//
if (BB > 90){
LCDBB = map(BB, 90, 170, 0, 10); // scale forward speed in percentage
}
if (BB < 90){
LCDBB = map(BB, 90, 10, 0, 10); // scale reverse speed in percentage
}
if (SB > 90){
LCDSB = map(SB, 90, 170, 0, 10); // scale forward speed in percentage
}
if (SB < 90){
LCDSB = map(SB, 90, 10, 0, 10); // scale reverse speed in percentage
}
if (RearT > 90){
LCDRear = map(RearT, 90, 170, 0, 10); // scale dive speed in percentage
}
if (RearT < 90){
LCDRear = map(RearT, 90, 10, 0, 10); // scale surface speed in percentage
}
lcd1.clear(); // clear display
lcd1.setCursor(8, 0); // place cursor row 1, collumn 1
lcd1.print("DIVE"); // write text
lcd1.setCursor(13, 0);
if ((LCDRear) < 100) {
lcd1.print(" "); // this writes a space to shove the number over one place if the number is 2 digits
if ((LCDRear) < 10) {
lcd1.print(" ");
}
}
lcd1.print(LCDRear); // now write the number
lcd1.setCursor(0, 0);
if (SS1 == HIGH){
lcd1.print("STATIC");
}
else{
lcd1.print("DYNAMIC");
}
lcd1.setCursor(0, 1); // place cursor row 2, collumn 1
lcd1.print("BB"); // write text
lcd1.setCursor(4, 1); // place cursor row 2, collumn 3
if ((LCDBB) < 100){
lcd1.print(" "); // this writes a space to shove the number over one place if the number is 2 digits
if ((LCDBB) < 10) {
lcd1.print(" "); // this writes another space to shove the number over a total of 2 spaces if less than 2 digits
}
}
lcd1.print(LCDBB); // write BB engine speed percentage
lcd1.setCursor(8, 1); // place cursor row 2, collumn 7
lcd1.print("SB"); // write text
lcd1.setCursor(13, 1); // place cursor row 2, collumn 9
if ((LCDSB) < 100) {
lcd1.print(" "); // this writes a space to shove the number over one place if the number is 2 digits
if ((LCDSB) < 10) {
lcd1.print(" "); // this writes another space to shove the number over a total of 2 spaces if less than 2 digits
}
}
lcd1.print(LCDSB); // write SB engine speed percentage // refresh in .5 seconds
// WRITE DISPLAY 2
lcd2.clear();
lcd2.setCursor(0,0);
lcd2.print("Heading:");
lcd2.setCursor(0,1);
lcd2.print("Depth:");
}