Code to drive motor controllers, and other misc.

Stellaris_Launchpad_Motor_Driver.ino

//J.Brian 1-22-2015
// inputString format:
// vert, port, stbd, aux1, aux2, aux3

#include <SerialBuffer.h>

// change this to Serial1 etc if you're using an Arduino Mega
#define SERIAL_PORT Serial

#define BUFFER_SIZE 64
byte buffer[BUFFER_SIZE];

// declare the serial buffer
SerialBuffer serialBuffer;

//#define DEBUG true
#define LED RED_LED

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;

//String inputString = "";
//boolean stringComplete = false;

byte message = 0x00;
byte getVert = 0x01;
byte getPort = 0x02;
byte getStbd = 0x03;

// inputString format:
// vert, port, stbd, aux1, aux2, aux3

//pc -> serial
//bbb -> serial5
//motors -> serial1

int vert = 0;
int port = 0;
int stbd = 0;

void setup() {
  // set up the buffer storage and maximum size
  serialBuffer.buffer = buffer;
  serialBuffer.bufferSize = BUFFER_SIZE;

  // reset the buffer
  serialBuffer.reset();
  
  // initialize both serial ports:
  //Serial1.setPins(UART1_PORTB);  //uncomment this line if the jumpers are moved to the alternate serial conn. for the bbb
  
  #ifdef DEBUG 
    Serial.begin(115200);
  #endif
  
  Serial5.begin(115200);
  //inputString.reserve(11);
  
  Serial1.begin(115200);
  pinMode(LED, OUTPUT);
}

void loop() {
  // read from port 5, send to port 1:
  int maxBytes = Serial5.available();
    while (maxBytes--) {
      byte inputByte = Serial5.read();       
      // (return value is message length)
      int bufferStatus = serialBuffer.receive(inputByte);
      if (bufferStatus >= 0) {
        #ifdef DEBUG
          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];
                      }
                    }
                  }
                }
              }
            }
          }
          Serial.print(vert);
          Serial.print(", ");
          Serial.print(port);
          Serial.print(", ");
          Serial.println(stbd);
        #endif
        //TODO: correct for the special characters!
        Serial1.write(START_MESSAGE);
        for (int i = 0; i < buffer[1]; i ++) {
          Serial1.write(buffer[i]);
        }
        Serial1.write(END_MESSAGE);
      } 
    }
}
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;
}