Skip to content

Instantly share code, notes, and snippets.

@hephaestus9

hephaestus9/Motor_Control_Port.ino

Last active August 29, 2015 14:13
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save hephaestus9/e93c5938cd7bd920e269 to your computer and use it in GitHub Desktop.
Save hephaestus9/e93c5938cd7bd920e269 to your computer and use it in GitHub Desktop.
Download ZIP
Code for Port side of ROV
Raw
Motor_Control_Port.ino
//J.Brian 1-22-2015
// inputString format:
// vert, port, stbd, aux1, aux2, aux3
// Control -> Forward:
// DIO 4, PWM 5 -> Forward
// DIO 2, PWM 3 -> Reverse
// Slave Control 1 -> Vert:
// DIO 7, PWM 6 -> Up
// DIO 8, PWM 9 -> Down
// Slave Control 2 -> Aft:
// DIO 12, PWM 10 -> Forward
// DIO 13, PWM 11 -> Reverse
#include <SerialBuffer.h>
#define BUFFER_SIZE 64
byte buffer[BUFFER_SIZE];
// declare the serial buffer
SerialBuffer serialBuffer;
// Define PORT STBD or AUX control board
#define PORTSIDE true
//#define STBDSIDE true
//#define AUX true
//debug option
#define DEBUG true
byte START_MESSAGE = 0x7F;
byte VERT = 0x76;
byte PORT = 0x70;
byte STBD = 0x73;
byte AUX1 = 0x61;
byte AUX2 = 0x62;
byte AUX3 = 0x63;
byte END_MESSAGE = 0x7E;
byte ESCAPE = 0x7D;
int TABLE[] = {
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
};
byte sum = 0x00;
boolean fin = false;
int controlDIO1 = 4;
int controlDIO2 = 2;
int controlPWM1 = 5;
int controlPWM2 = 3;
int slave1DIO1 = 7;
int slave1DIO2 = 8;
int slave1PWM1 = 6;
int slave1PWM2 = 9;
int slave2DIO1 = 12;
int slave2DIO2 = 13;
int slave2PWM1 = 10;
int slave2PWM2 = 11;
int vert = 0;
int prevVert = 0;
int port = 0;
int prevPort = 0;
int stbd = 0;
int prevStbd = 0;
void setup() {
serialBuffer.buffer = buffer;
serialBuffer.bufferSize = BUFFER_SIZE;
// reset the buffer
serialBuffer.reset();
Serial.begin(115200);
pinMode(controlDIO1, OUTPUT);
pinMode(controlDIO2, OUTPUT);
pinMode(controlPWM1, OUTPUT);
pinMode(controlPWM2, OUTPUT);
pinMode(slave1DIO1, OUTPUT);
pinMode(slave1DIO2, OUTPUT);
pinMode(slave1PWM1, OUTPUT);
pinMode(slave1PWM2, OUTPUT);
pinMode(slave2DIO1, OUTPUT);
pinMode(slave2DIO2, OUTPUT);
pinMode(slave2PWM1, OUTPUT);
pinMode(slave2PWM2, OUTPUT);
}
void loop() {
delay(100);
int maxBytes = Serial.available();
while (maxBytes--) {
byte inputByte = Serial.read();
// (return value is message length)
int bufferStatus = serialBuffer.receive(inputByte);
if (bufferStatus >= 0) {
fin = false;
digest();
while(!fin);
/*Serial.print(sum);
Serial.print(", ");
Serial.print(buffer[0]);
Serial.print(", ");
Serial.println(buffer[1]);*/
if ( sum == buffer[0]) {
if (buffer[2] == VERT) {
if ((int)buffer[3] + (int)buffer[4] == 498) {
vert = (-1 * (int)buffer[5]);
if (buffer[6] == PORT) {
if ((int)buffer[7] + (int)buffer[8] == 498) {
port = (-1 * (int)buffer[9]);
if (buffer[10] == STBD) {
if ((int)buffer[11] + (int)buffer[12] == 498) {
stbd = (-1 * (int)buffer[13]);
}
else {
stbd = (int)buffer[11];
}
}
}
else {
port = (int)buffer[7];
if (buffer[8] == STBD) {
if ((int)buffer[9] + (int)buffer[10] == 498) {
stbd = (-1 * (int)buffer[11]);
}
else {
stbd = (int)buffer[9];
}
}
}
}
}
else {
vert = (int)buffer[3];
if (buffer[4] == PORT) {
if ((int)buffer[5] + (int)buffer[6] == 498) {
port = (-1 * (int)buffer[7]);
if (buffer[8] == STBD) {
if ((int)buffer[9] + (int)buffer[10] == 498) {
stbd = (-1 * (int)buffer[11]);
}
else {
stbd = (int)buffer[12];
}
}
}
else {
port = (int)buffer[5];
if (buffer[6] == STBD) {
if ((int)buffer[7] + (int)buffer[8] == 498) {
stbd = (-1 * (int)buffer[9]);
}
else {
stbd = (int)buffer[7];
}
}
}
}
}
}
}
#ifdef DEBUG
Serial.print(vert);
Serial.print(", ");
Serial.print(port);
Serial.print(", ");
Serial.println(stbd);
#endif
Vert();
#ifdef PORTSIDE
Port();
#endif
#ifdef STBDSIDE
Stbd();
#endif
}
}
}
void update(byte b){
sum = TABLE[sum^b];
}
void digest(){
fin = false;
sum = 0x00;
for (int i = 2; i < buffer[1]; i++) {
update(buffer[i]);
}
fin = true;
}
void Port(){
if (port != prevPort){
if (port > 0 && prevPort < 0){
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, LOW);
analogWrite(controlPWM1, 0);
analogWrite(controlPWM2, 255);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, LOW);
analogWrite(slave2PWM1, 255);
analogWrite(slave2PWM2, 0);
delay(250);
}
if (port < 0 && prevPort > 0){
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, LOW);
analogWrite(controlPWM1, 255);
analogWrite(controlPWM2, 0);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, LOW);
analogWrite(slave2PWM1, 0);
analogWrite(slave2PWM2, 255);
delay(250);
}
if (port > 0){
digitalWrite(controlDIO1, HIGH);
digitalWrite(controlDIO2, LOW);
analogWrite(controlPWM1, port);
analogWrite(controlPWM2, 0);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, HIGH);
analogWrite(slave2PWM1, 0);
analogWrite(slave2PWM2, port);
//Serial.println(port);
}
if (port < 0){
//Serial.println(port);
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, HIGH);
analogWrite(controlPWM1, 0);
analogWrite(controlPWM2, (-1 * port));
digitalWrite(slave2DIO1, HIGH);
digitalWrite(slave2DIO2, LOW);
analogWrite(slave2PWM1, (-1 * port));
analogWrite(slave2PWM2, 0);
}
if (port == 0){
//Serial.println(port);
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, LOW);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, LOW);
if (prevPort > 0){
analogWrite(controlPWM1, 255);
analogWrite(slave2PWM2, 255);
delay(250);
}
else{
analogWrite(controlPWM2, 255);
analogWrite(slave2PWM1, 255);
delay(250);
}
analogWrite(controlPWM1, 0);
analogWrite(controlPWM2, 0);
analogWrite(slave2PWM1, 0);
analogWrite(slave2PWM2, 0);
}
prevPort = port;
}
}
void Stbd() {
if (stbd != prevStbd){
if (stbd > 0 && prevStbd < 0){
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, LOW);
analogWrite(controlPWM1, 0);
analogWrite(controlPWM2, 255);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, LOW);
analogWrite(slave2PWM1, 255);
analogWrite(slave2PWM2, 0);
delay(250);
}
if (stbd < 0 && prevStbd > 0){
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, LOW);
analogWrite(controlPWM1, 255);
analogWrite(controlPWM2, 0);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, LOW);
analogWrite(slave2PWM1, 0);
analogWrite(slave2PWM2, 255);
delay(250);
}
if (stbd > 0){
digitalWrite(controlDIO2, LOW);
digitalWrite(controlDIO1, HIGH);
analogWrite(controlPWM1, stbd);
analogWrite(controlPWM2, 0);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, HIGH);
analogWrite(slave2PWM1, 0);
analogWrite(slave2PWM2, stbd);
//Serial.println(stbd);
}
if (stbd < 0){
//Serial.println(stbd);
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, HIGH);
analogWrite(controlPWM1, 0);
analogWrite(controlPWM2, (-1 * stbd));
digitalWrite(slave2DIO2, LOW);
digitalWrite(slave2DIO1, HIGH);
analogWrite(slave2PWM1, (-1 * stbd));
analogWrite(slave2PWM2, 0);
}
if (stbd == 0){
//Serial.println(stbd);
digitalWrite(controlDIO1, LOW);
digitalWrite(controlDIO2, LOW);
digitalWrite(slave2DIO1, LOW);
digitalWrite(slave2DIO2, LOW);
if (prevStbd > 0){
analogWrite(controlPWM1, 255);
analogWrite(slave2PWM2, 255);
delay(250);
}
else{
analogWrite(controlPWM2, 255);
analogWrite(slave2PWM1, 255);
delay(250);
}
analogWrite(controlPWM1, 0);
analogWrite(controlPWM2, 0);
analogWrite(slave2PWM1, 0);
analogWrite(slave2PWM2, 0);
}
prevStbd = stbd;
}
}
void Vert(){
if (vert != prevVert){
if (vert > 0 && prevVert < 0){
digitalWrite(slave1DIO1, LOW);
digitalWrite(slave1DIO2, LOW);
analogWrite(slave1PWM1, 0);
analogWrite(slave1PWM2, 255);
delay(250);
}
if (vert < 0 && prevVert > 0){
digitalWrite(slave1DIO1, LOW);
digitalWrite(slave1DIO2, LOW);
analogWrite(slave1PWM1, 255);
analogWrite(slave1PWM2, 0);
delay(250);
}
if (vert > 0){
digitalWrite(slave1DIO1, HIGH);
digitalWrite(slave1DIO2, LOW);
analogWrite(slave1PWM1, vert);
analogWrite(slave1PWM2, 0);
}
if (vert < 0){
digitalWrite(slave1DIO1, LOW);
digitalWrite(slave1DIO2, HIGH);
analogWrite(slave1PWM1, 0);
analogWrite(slave1PWM2, (-1 * vert));
}
if (vert == 0){
digitalWrite(slave1DIO1, LOW);
digitalWrite(slave1DIO2, LOW);
if (prevVert > 0){
analogWrite(slave1PWM1, 255);
delay(250);
}
else{
analogWrite(slave1PWM2, 255);
delay(250);
}
analogWrite(slave1PWM1, 0);
analogWrite(slave1PWM2, 0);
}
prevVert = vert;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment