// This is the subsea ROV code for Sigbjørns ROV project. Remember the library pack!
#include <SPI.h> // needed for Arduino versions later than 0018
#include <Ethernet.h>
#include <EthernetUdp.h> // UDP library from:
bjoern@cs.stanford.edu 12/30/2008 This is altered to support 100 bytes of data
#include <Servo.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <Adafruit_BMP085.h>
Servo escservo0;
Servo escservo1;
Servo escservo2;
Servo escservo3;
using namespace std;
// Store our compass as a variable.
HMC5883L compass;
// Pressure and temp sensor
Adafruit_BMP085 bmp;
// Record any errors that may occur in the compass.
int error = 0;
#define NO_SERVOS 4
#define DEFAULT_SERVO_VALUE 512
#define LENGTH_OF_SERVOVALUE 4
#define LENGTH_OF_TEXT 5
static const float declinationAngle = 0.03869; // Magnetic deviation Trondheim
// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {
0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
IPAddress ip(192, 168, 0, 2);
IPAddress ipRem(192, 168, 0, 1);
unsigned int localPort = 8888; // local port to listen on
// buffers for receiving and sending data
char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet
// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;
int packetcount;
void setup() {
packetcount = 0,
// start the Ethernet and UDP:
Ethernet.begin(mac,ip);
bmp.begin();
Udp.begin(localPort);
escservo0.attach(3, 800, 1700); // Port 3, min, max
escservo1.attach(5, 800, 1700); // Port 5, min, max
escservo2.attach(6, 800, 1700); // Port 6, min, max
escservo3.attach(9, 800, 1700); // Port 9, min, max
escservo0.write(90);
escservo1.write(90);
escservo2.write(90);
escservo3.write(90);
Serial.begin(9600);
Wire.begin(); // Start the I2C interface.
compass = HMC5883L(); // Construct a new HMC5883 compass.
}
int heading, temp, pressure;
MagnetometerScaled scaled;
void loop() {
if (packetcount == 30) {
packetcount = 0;
// Retrived the scaled values from the compass (scaled to the configured scale).
scaled = compass.ReadScaledAxis();
float heading_f = atan2(scaled.YAxis, scaled.XAxis);
heading_f += declinationAngle;
// Correct for when signs are reversed.
if(heading_f < 0){
heading_f += 2*PI;
}
// Check for wrap due to addition of declination.
if(heading_f > 2*PI){
heading_f -= 2*PI;
}
heading = heading_f * 180/M_PI;
temp = (int)bmp.readTemperature();
pressure = bmp.readPressure();
char replyBuffer[100] = {
'\0' }; // a string to send back
sprintf(replyBuffer, "heading:%i, temp:%i, pressure:%i \n", heading, temp, pressure);
Udp.beginPacket(ipRem, localPort);
Udp.write(replyBuffer);
Udp.endPacket();
}
// if there's data available, read a packet
packetBuffer[20] = NULL;
int packetSize = Udp.parsePacket();
if(packetSize){ // If we recieved a packet
packetcount++;
// read the packet into packetBufffer
Udp.read(packetBuffer,20);
//Serial.println(packetBuffer);
int servoValues[NO_SERVOS] = {
DEFAULT_SERVO_VALUE };
//Serial.print(packetBuffer);
for(int i=0; i<NO_SERVOS; i++){ // Search for all the strings we are looking for
char subbuff[5];
memcpy( subbuff, packetBuffer+(i*4),4);
subbuff[4] = '\0';
//Serial.println(subbuff);
servoValues
= atoi(subbuff);
}
for(int i=0; i<NO_SERVOS; i++){
if (servoValues > 980) servoValues = 980;
}
escservo0.write(servoValues[0]/7);
escservo1.write(servoValues[1]/7);
escservo2.write(servoValues[2]/7);
escservo3.write(servoValues[3]/7);
}
}