------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
MotorControl.pde
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------#include "EasyTransfer.h"
#include "IOpinsROV.h"
//create object for recieving bytes.
EasyTransfer ET;
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
int left;
int right;
int up;
int arm;
int brake;
};
//give a name to the group of data
RECEIVE_DATA_STRUCTURE mydata;
int Leftmode=1; // 0=reverse, 1=brake, 2=forward
int Rightmode=1; // 0=reverse, 1=brake, 2=forward
int Upmode=1;
int LeftPWM; // PWM value for left motor speed / brake
int RightPWM; // PWM value for right motor speed / brake
int UpPWM;
void setup(){
Serial.begin(9600);
//start the library, pass in the data details and the name of the serial port. Can be Serial, Serial1, Serial2, etc.
ET.begin(details(mydata), &Serial);
pinMode(13, OUTPUT);
}
void loop(){
//check and see if a data packet has come in.
if(ET.receiveData()){
//this is how you access the variables. [name of the group].[variable name]
//since we have data, we will blink it out.
if (mydata.brake = 1) {
analogWrite(LmotorA,min(abs(mydata.brake),255));
analogWrite(LmotorB,min(abs(mydata.brake),255));
analogWrite(RmotorA,min(abs(mydata.brake),255));
analogWrite(RmotorB,min(abs(mydata.brake),255));
analogWrite(UmotorA,min(abs(mydata.brake),255));
analogWrite(UmotorB,min(abs(mydata.brake),255));
break;
} else {
if (mydata.left <= -50) {
analogWrite(LmotorA,min(abs(mydata.left),255));
analogWrite(LmotorB,0);
break;
} else if (mydata.left >= 50) {
analogWrite(LmotorA,0);
analogWrite(LmotorB,min(abs(mydata.left),255));
break;
} else {
analogWrite(LmotorA,0);
analogWrite(LmotorB,0);
break;
}
if (mydata.right <= -50) {
analogWrite(RmotorA,min(abs(mydata.right),255));
analogWrite(RmotorB,0);
break;
} else if (mydata.right >= 50) {
analogWrite(RmotorA,0);
analogWrite(RmotorB,min(abs(mydata.right),255));
break;
} else {
analogWrite(RmotorA,0);
analogWrite(RmotorB,0);
break;
}
if (mydata.up <= -50) {
analogWrite(UmotorA,min(abs(mydata.up),255));
analogWrite(UmotorB,0);
break;
} else if (mydata.up >= 50) {
analogWrite(UmotorA,0);
analogWrite(UmotorB,min(abs(mydata.up),255));
break;
} else {
analogWrite(UmotorA,0);
analogWrite(UmotorB,0);
break;
}
}
switch (mydata.arm) {
case 0: //"arm close"
analogWrite(ArmmotorA,200);
analogWrite(ArmmotorB,0);
break;
case 1: //"arm brake"
analogWrite(ArmmotorA,200);
analogWrite(ArmmotorB,200);
break;
case 2: //"arm open"
analogWrite(ArmmotorA,0);
analogWrite(ArmmotorB,200);
break;
default:
analogWrite(ArmmotorA,0);
analogWrite(ArmmotorB,0);
break;
}
}
//you should make this delay shorter then your transmit delay or else messages could be lost
delay(150);
}
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
EasyTransfer.cpp
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------#include "EasyTransfer.h"
//Captures address and size of struct
void EasyTransfer::begin(uint8_t * ptr, uint8_t length, HardwareSerial *theSerial){
address = ptr;
size = length;
_serial = theSerial;
//dynamic creation of rx parsing buffer in RAM
rx_buffer = (uint8_t*) malloc(size);
}
//Sends out struct in binary, with header, length info and checksum
void EasyTransfer::sendData(){
uint8_t CS = size;
_serial->write(0x06);
_serial->write(0x85);
_serial->write(size);
for(int i = 0; i<size; i++){
CS^=*(address+i);
_serial->write(*(address+i));
}
_serial->write(CS);
}
boolean EasyTransfer::receiveData(){
//start off by looking for the header bytes. If they were already found in a previous call, skip it.
if(rx_len == 0){
//this size check may be redundant due to the size check below, but for now I'll leave it the way it is.
if(_serial->available() >= 3){
//this will block until a 0x06 is found or buffer size becomes less then 3.
while(_serial->read() != 0x06) {
//This will trash any preamble junk in the serial buffer
//but we need to make sure there is enough in the buffer to process while we trash the rest
//if the buffer becomes too empty, we will escape and try again on the next call
if(_serial->available() < 3)
return false;
}
if (_serial->read() == 0x85){
rx_len = _serial->read();
//make sure the binary structs on both Arduinos are the same size.
if(rx_len != size){
rx_len = 0;
return false;
}
}
}
}
//we get here if we already found the header bytes, the struct size matched what we know, and now we are byte aligned.
if(rx_len != 0){
while(_serial->available() && rx_array_inx <= rx_len){
rx_buffer[rx_array_inx++] = _serial->read();
}
if(rx_len == (rx_array_inx-1)){
//seem to have got whole message
//last uint8_t is CS
calc_CS = rx_len;
for (int i = 0; i<rx_len; i++){
calc_CS^=rx_buffer[i];
}
if(calc_CS == rx_buffer[rx_array_inx-1]){//CS good
memcpy(address,rx_buffer,size);
rx_len = 0;
rx_array_inx = 0;
return true;
}
else{
//failed checksum, need to clear this out anyway
rx_len = 0;
rx_array_inx = 0;
return false;
}
}
}
return false;
}
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
EasyTransfer.h
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------#ifndef EasyTransfer_h
#define EasyTransfer_h
//make it a little prettier on the front end.
#define details(name) (byte*)&name,sizeof(name)
//Not neccessary, but just in case.
#if ARDUINO > 22
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "HardwareSerial.h"
//#include <NewSoftSerial.h>
#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <avr/io.h>
class EasyTransfer {
public:
void begin(uint8_t *, uint8_t, HardwareSerial *theSerial);
//void begin(uint8_t *, uint8_t, NewSoftSerial *theSerial);
void sendData();
boolean receiveData();
private:
HardwareSerial *_serial;
//NewSoftSerial *_serial;
uint8_t * address; //address of struct
uint8_t size; //size of struct
uint8_t * rx_buffer; //address for temporary storage and parsing buffer
uint8_t rx_array_inx; //index for RX parsing buffer
uint8_t rx_len; //RX packet length according to the packet
uint8_t calc_CS; //calculated Chacksum
};
#endif
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
IOPinsCB.h
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------#define LmotorA 3 // Left motor H bridge, input A
#define LmotorB 11 // Left motor H bridge, input B
#define RmotorA 5 // Right motor H bridge, input A
#define RmotorB 6 // Right motor H bridge, input B
#define UmotorA 5 // Up motor H bridge, input A
#define UmotorB 6 // Up motor H bridge, input B
#define ArmmotorA 5 // Up motor H bridge, input A
#define ARmmotorB 6 // Up motor H bridge, input B
