Post by RC Tractor Guy on Apr 28, 2015 22:44:08 GMT
I was asked for the code I'm using in a tractor with two Arduinos, one in the cab and one in the body. The only model I have working is the John Deere 8360RT model so I'm going to post that code here.
Be warned this is not a finalised code, it does have bugs, most features haven't been implemented and a lot of the comments are meaningless because I just copied and pasted from my older code. The body code in particular needs considerable work, I haven't looked at it since last november. However the John Deere does drive well and has good control of the lifting arms.
Controller Code:
Tractor Cab Code:
Body Code:
There is a long way to go with this code but I'm getting there.
Be warned this is not a finalised code, it does have bugs, most features haven't been implemented and a lot of the comments are meaningless because I just copied and pasted from my older code. The body code in particular needs considerable work, I haven't looked at it since last november. However the John Deere does drive well and has good control of the lifting arms.
Controller Code:
// Define variables
int Left_Joy_X = 0x00;
int Left_Joy_Y = 0x00;
int Left_Pot = 0x00;
int Right_Joy_X = 0x00;
int Right_Joy_Y = 0x00;
int Right_Pot = 0x00;
int Left_Joy_X_Old = 0x00;
int Left_Joy_Y_Old = 0x00;
int Left_Pot_Old = 0x00;
int Right_Joy_X_Old = 0x00;
int Right_Joy_Y_Old = 0x00;
int Right_Pot_Old = 0x00;
int Push_Buttons = 0x00;
int Data_Check_Timer = 0;
int DIP1 = LOW;
int DIP2 = LOW;
int DIP3 = LOW;
int DIP4 = LOW;
int DIP5 = LOW;
int Left_Joy_Button = LOW;
int Right_Joy_Button = LOW;
int Menu = 0;
int Indicators = 0;
int Head_Lights = 0;
int Work_Lights = 0;
int Indicators_Old = 0;
int Head_Lights_Old = 0;
int Work_Lights_Old = 0;
#include <SPI.h>
#include <RH_NRF24.h>
// Singleton instance of the radio driver
RH_NRF24 nrf24;
// RH_NRF24 nrf24(8, 53); // use this to be electrically compatible with Mirf
// RH_NRF24 nrf24(8, 10);// For Leonardo, need explicit SS pin
// RH_NRF24 nrf24(8, 7); // For RFM73 on Anarduino Mini
/*-----( Declare Variables )-----*/
uint8_t command[16]; // 2 element array of unsigned 8-bit type, holding Joystick readings
// Predefine the message buffer here: Don't put this on the stack:
uint8_t buf[RH_NRF24_MAX_MESSAGE_LEN]; // Actually: 28 bytes (32 minus 4 byte header)
void setup()
{
pinMode(2, INPUT_PULLUP); // Dip switch 1
pinMode(3, INPUT_PULLUP); // Dip switch 2
pinMode(4, INPUT_PULLUP); // Dip switch 3
pinMode(5, INPUT_PULLUP); // Dip switch 4
pinMode(6, INPUT_PULLUP); // Dip switch 5
pinMode(7, INPUT_PULLUP); // Left joystick button
pinMode(9, INPUT_PULLUP); // Right joystick button
command[0] = 0x4A;
command[1] = 0x44;
command[2] = 0x38;
command[3] = 0x33;
//command[4]=0;
//command[5]=0;
//command[6]=0;
//command[7]=0;
//command[8]=0;
//command[9]=0;
//command[10]=0;
//command[11]=0;
//command[12]=0;
// initialize the serial communication:
Serial.begin(9600);
nrf24.init();
nrf24.setChannel(1);
nrf24.setRF(RH_NRF24::DataRate2Mbps, RH_NRF24::TransmitPower0dBm);
}
void loop() {
Check_Vehicle_ID();
Check_Buttons();
Check_Pots();
Regular_Data_Check();
delay(100);
}
void Check_Vehicle_ID(){
// Check Dip Switches
DIP1=digitalRead(2);
DIP2=digitalRead(3);
DIP3=digitalRead(4);
DIP4=digitalRead(5);
DIP5=digitalRead(6);
if(DIP1==LOW && DIP2==LOW && DIP3==LOW && DIP4==LOW && DIP5==LOW){
command[0] = 0x4A; // J
command[1] = 0x44; // D
command[2] = 0x38; // 8
command[3] = 0x33; // 3
}
}
void Check_Buttons(){
Push_Buttons = analogRead(A6); // read the value from the sensor
if (Push_Buttons < 50){ // Button 1
if (command[10] == 0){
command[10] = 1;
}
else {
command[10] = 0;
}
while(Push_Buttons < 50){
Push_Buttons = analogRead(A6);
}
}
if (Push_Buttons > 50 && Push_Buttons < 250){ // Button 2
if(command[11] == 0){
command[11] = 1;
}
else{
command[11] = 0;
}
while(Push_Buttons > 50 && Push_Buttons < 250){
Push_Buttons = analogRead(A6);
}
}
if (Push_Buttons > 500 && Push_Buttons < 700){ // Button 3
if(command[13] == 0){
command[13] = 1;
}
else{
command[13] = 0;
}
while(Push_Buttons > 500 && Push_Buttons < 700){
Push_Buttons = analogRead(A6);
}
}
if (Push_Buttons > 700 && Push_Buttons < 900){ // Button 4
if(command[14] == 0){
command[14] = 1;
}
else{
command[14] = 0;
}
while(Push_Buttons > 450 && Push_Buttons < 650){
Push_Buttons = analogRead(A6);
}
}
// Check Joystick Switches
Left_Joy_Button = digitalRead(7);
Right_Joy_Button = digitalRead(9);
if (Left_Joy_Button==LOW && Right_Joy_Button==LOW)
{
if(command[12]!=3){
command[12]=3; // Turn on Hazard Lights
}
else{
command[12]=0; // Turn off Indicators
}
// Wait for switch to be released
while (Left_Joy_Button == LOW || Right_Joy_Button==LOW)
{
Left_Joy_Button = digitalRead(7);
Right_Joy_Button = digitalRead(9);
}
Send_Data();
}
else if (Right_Joy_Button==LOW)
{
if (command[12] != 1){
command[12]=1; // Command to turn on right indicator
}
else {
command[12]=0; // Turn off Indicators
}
// Wait for switch to be released
while (Right_Joy_Button == LOW)
{
Right_Joy_Button = digitalRead(9);
}
Send_Data();
}
else if (Left_Joy_Button==LOW)
{
if (command[12] != 2){
command[12]=2; // Command to turn on right indicator
}
else {
command[12]=0; // Turn off Indicators
}
// Wait for switch to be released
while (Left_Joy_Button == LOW)
{
Left_Joy_Button = digitalRead(7);
}
Send_Data();
}
}
void Check_Pots(){
Left_Joy_X = analogRead(A3);
Left_Joy_X= map(Left_Joy_X, 0, 1024, 255, 0);
Left_Joy_Y = analogRead(A2);
Left_Joy_Y= map(Left_Joy_Y, 0, 1024, 255, 0);
Left_Pot = analogRead(A4);
Left_Pot= map(Left_Pot, 0, 1024, 255, 0);
Right_Joy_X = analogRead(A1);
Right_Joy_X= map(Right_Joy_X, 0, 1024, 255, 0);
Right_Joy_Y = analogRead(A0);
Right_Joy_Y= map(Right_Joy_Y, 0, 1024, 0, 255);
Right_Pot = analogRead(A5);
Right_Pot= map(Right_Pot, 0, 1024, 255, 0);
if(Left_Joy_X != Left_Joy_X_Old || Left_Joy_Y != Left_Joy_Y_Old || Left_Pot != Left_Pot_Old || Right_Joy_X != Right_Joy_X_Old || Right_Joy_Y != Right_Joy_Y_Old || Right_Pot != Right_Pot_Old){
command[4]=Right_Joy_Y;
command[5]=Left_Joy_X;
command[6]=Left_Pot;
command[7]=Right_Joy_X;
command[8]=Left_Joy_Y;
command[9]=Right_Pot;
Send_Data();
Left_Joy_X_Old=Left_Joy_X;
Left_Joy_Y_Old=Left_Joy_Y;
Left_Pot_Old=Left_Pot;
Right_Joy_X_Old=Right_Joy_X;
Right_Joy_Y_Old=Right_Joy_Y;
Right_Pot_Old=Right_Pot;
}
}
// Function to send the data
void Send_Data(){
uint8_t buf[RH_NRF24_MAX_MESSAGE_LEN];
uint8_t len = sizeof(buf);
nrf24.send(command, sizeof(command));
nrf24.waitPacketSent();
Data_Check_Timer = 0;
}
// Send data in regular intervals to prevent tractor loosing control
void Regular_Data_Check(){
Data_Check_Timer=Data_Check_Timer+1;
if(Data_Check_Timer=1024){
Send_Data();
}
}
Tractor Cab Code:
/*
Generic RC Tractor Cab Code Version 1.0
This code was written by Oisin O'Conchubhair for the website www.rctractors.net
This is the generic code used to control homemade RC Tractors.
The code is intended to be used with an Arduino Pro Mini
Circuit diagram can be found ar www.rctractors.net
Pin Assignment
D0 Serial RX
D1 Serial TX
D2 Right Indicator
D3 Work Lights
D4 Right Beacon
D5
D6
D7 Left Beacon
D8 NRF24 CE
D9 Left Indicator
D10 NRF24 CSN
D11 NRF24 MOSI
D12 NRF24 MISO
D13 NRF24 SCK
A0
A1
A2 Not assigned
A3 Not assigned
A4 Not assigned
A5 Not assigned
*/
// This section describes the vehicle
byte ID1 = 0x4A; // Enter a four byte ID for the vehicle
byte ID2 = 0x44; // for example F936 for the Fendt 936 model
byte ID3 = 0x38; // would be ID1 = 0x46; ID2 = 0x39; ID3 = 0x33; ID4 = 0x36
byte ID4 = 0x33; // because asci character F is 46 in hexidecimal and so on
// Variables used in the program
int Active = 0; // Variable to determine if the tractor is in active
int left_ind = 0; // Variable indicating if the left indicator is currently on or off
int right_ind = 0; // Variable indicating if the right indicator is currently on or off
int Ind_Timer = 0; // Variable used to record the length of time the indicator is on for
int ind_timer2 = 0; // Variable used to record the beacon is on for
int beac_on = 0; // Variable indicating if the right indicator is currently on or off
int beac_timer = 0; // Variable used to record the length of time the indicator is on for
int ind_active = 0; // Variable set to show that indicator code should be running
int beacon_active = 0; // Variable set to show that indicator code should be running
int spots_on = 0; // Variable to idicate if the large spot lights are on
int Last_Data_Timer = 0;
int Beacon_Option=2;
int Beacon_Timer=0;
byte dat1 = 0x00; // Stores received byte
byte dat2 = 0x00; // Stores received byte
byte dat3 = 0x00; // Stores received byte
byte dat4 = 0x00; // Stores received byte
byte dat5 = 0x00; // Stores received byte
byte dat6 = 0x00; // Stores received byte
byte command_val = 0; // Stores the command value
byte Data_Val_1 = 0; // Stores the data value
byte Data_Val_2 = 0; // Stores the data value
byte Data_Val_3 = 0; // Stores the data value
byte Data_Val_4 = 0; // Stores the data value
byte Data_Val_5 = 0; // Stores the data value
byte Data_Val_6 = 0; // Stores the data value
byte Data_Val_7 = 0; // Stores the data value
byte Data_Val_8 = 0; // Stores the data value
byte Data_Val_9 = 0; // Stores the data value
byte Data_Val_10 = 0; // Stores the data value
byte Data_Val_11 = 0; // Stores the data value
byte Data_Val_12 = 0; // Stores the data value
int data = 0; // Variable to indicate serial data is available
#include <SPI.h>
#include <RH_NRF24.h>
RH_NRF24 nrf24; // Create NRF24 object
void setup()
{
Serial.begin(9600);
pinMode(2, OUTPUT); // Right indicator
pinMode(3, OUTPUT); // Work lights
pinMode(4, OUTPUT); // Right Beacon
pinMode(7, OUTPUT); // Left Beacon
pinMode(9, OUTPUT); // Left indicator
nrf24.init(); // Initiailize NRF24 radio module
nrf24.setChannel(1); // Defaults after init are 2.402 GHz (channel 2), 2Mbps, 0dBm
nrf24.setRF(RH_NRF24::DataRate2Mbps, RH_NRF24::TransmitPower0dBm);
}
void loop()
{
Data_Check();
Data_Loss_Check();
if(Active==1){
Update_Motors();
LED_Control();
Beacon_Control();
}
Ind_Control();
}
void Data_Check(){
if (nrf24.available())
{
// Should be a message for us now
uint8_t buf[16];
uint8_t len = sizeof(buf);
if (nrf24.recv(buf, &len))
{
// We recieved a string of ten bytes, four ID, one command and six data values but they may not be for this tractor
if (buf[0] == ID1 && buf[1] == ID2 && buf[2] == ID3 && buf[3] == ID4){ // Confirm that the correct vehicle ID has been recieved
Data_Val_1 = buf[4]; // Store the new data value
Data_Val_2 = buf[5]; // Store the new data value
Data_Val_3 = buf[6]; // Store the new data value
Data_Val_4 = buf[7]; // Store the new data value
Data_Val_5 = buf[8]; // Store the new data value
Data_Val_6 = buf[9]; // Store the new data value
Data_Val_7 = buf[10]; // Store the new data value
Data_Val_8 = buf[11]; // Store the new data value
Data_Val_9 = buf[12]; // Store the new data value
Data_Val_10 = buf[13]; // Store the new data value
Data_Val_11 = buf[14]; // Store the new data value
Data_Val_12 = buf[15]; // Store the new data value
Last_Data_Timer = 10;
Active = 1;
}
}
}
}
void Update_Motors(){
Serial.write(0x0A); // \n
Serial.write(0x02); // \n
Serial.write(Data_Val_1); // \n
delay(5);
Serial.write(0x0A); // \n
Serial.write(0x03); // \n
Serial.write(Data_Val_2); // \n
delay(5);
Serial.write(0x0A); // \n
Serial.write(0x0C); // \n
Serial.write(Data_Val_3); // \n
delay(5);
Serial.write(0x0A); // \n
Serial.write(0x0D); // \n
Serial.write(Data_Val_4); // \n
delay(5);
Serial.write(0x0A); // \n
Serial.write(0x01); // \n
Serial.write(Data_Val_5); // \n
delay(5);
Serial.write(0x0A); // \n
Serial.write(0x04); // \n
Serial.write(Data_Val_6); // \n
delay(5);
}
void LED_Control(){
if(Data_Val_8 == 1){
digitalWrite(3, HIGH); // turn it on
// Serial.write(0x0A); // \n
//Serial.write(0x07); // \n
//Serial.write(0x00); // \n
}
else{
digitalWrite(3, LOW); // turn it on
//Serial.write(0x0A); // \n
// Serial.write(0x07); // \n
//Serial.write(0x00); // \n
}
if(Data_Val_7 == 1){
//Serial.write(0x0A); // \n
// Serial.write(0x06); // \n
// Serial.write(0x00); // \n
}
else{
// Serial.write(0x0A); // \n
// Serial.write(0x06); // \n
// Serial.write(0x00); // \n
}
}
void Ind_Control(){
Ind_Timer = Ind_Timer+1;
if(Ind_Timer = 1000){
if (Data_Val_9 == 1){
ind_left();
}
else if(Data_Val_9 == 2){
ind_right();
}
else if(Data_Val_9 == 3){
ind_hazard();
}
else{
ind_off();
}
Ind_Timer = 0;
}
}
// Indicator control functions
void ind_left(){
if (left_ind == 0){ // If left indicator is off
digitalWrite(2, HIGH); // turn it on
Serial.write(0x0A); // \n
delay(5);
Serial.write(0x08); // \n
delay(5);
Serial.write(0x02); // \n
delay(5);
left_ind = 1;
}
else{ // If left indicator is on
digitalWrite(2, LOW); // turn it off
Serial.write(0x0A); // \n
delay(5);
Serial.write(0x08); // \n
delay(5);
Serial.write(0x00); // \n
delay(5);
left_ind = 0;
}
}
void ind_right(){
if (right_ind == 0){ // If right indicator is off
digitalWrite(9, HIGH); // turn it on
Serial.write(0x0A); // \n
delay(5);
Serial.write(0x08); // \n
delay(5);
Serial.write(0x01); // \n
delay(5);
right_ind = 1;
}
else{ // If right indicator is on
digitalWrite(9, LOW); // turn it off
Serial.write(0x0A); // \n
delay(5);
Serial.write(0x08); // \n
delay(5);
Serial.write(0x00); // \n
delay(5);
right_ind = 0;
}
}
void ind_hazard(){
if (left_ind == 0){ // If hazards are off
digitalWrite(2, HIGH); // turn them on
digitalWrite(9, HIGH);
Serial.write(0x0A); // \n
delay(5);
Serial.write(0x08); // \n
delay(5);
Serial.write(0x03); // \n
delay(5);
left_ind = 1;
}
else{ // If hazards are on
digitalWrite(2, LOW); // turn them off
digitalWrite(9, LOW);
Serial.write(0x0A); // \n
delay(5);
Serial.write(0x08); // \n
delay(5);
Serial.write(0x00); // \n
delay(5);
left_ind = 0;
}
}
void ind_off(){
}
void Beacon_Control(){
if(Beacon_Option==1){
Beacon_Timer++;
if (Beacon_Timer == 4){
digitalWrite(4, LOW);
digitalWrite(7, HIGH);
}
else if (Beacon_Timer == 8){
digitalWrite(4, HIGH);
digitalWrite(7, LOW);
Beacon_Timer = 0;
}
}
else if(Beacon_Option==2){
Beacon_Timer++;
if (Beacon_Timer == 4){
digitalWrite(4, HIGH);
}
if (Beacon_Timer == 6){
digitalWrite(4, LOW);
}
if (Beacon_Timer == 8){
digitalWrite(4, HIGH);
}
if (Beacon_Timer == 10){
digitalWrite(4, LOW);
}
if (Beacon_Timer == 14){
digitalWrite(7, HIGH);
}
if (Beacon_Timer == 16){
digitalWrite(7, LOW);
}
if (Beacon_Timer == 18){
digitalWrite(7, HIGH);
}
if (Beacon_Timer == 20){
digitalWrite(7, LOW);
Beacon_Timer = 0;
}
}
else{
digitalWrite(4, LOW);
digitalWrite(7, LOW);
}
}
void Data_Loss_Check(){
if(Last_Data_Timer != 0){
Last_Data_Timer = Last_Data_Timer - 1;
}
if(Last_Data_Timer == 0 && Active == 1){
Active = 0;
Data_Val_9 == 3;
Beacon_Option=0;
LED_Control();
Beacon_Control();
}
}
Body Code:
/*
Generic RC Tractor Body Code Version 1.0
This code was written by Oisin O'Conchubhair for the website www.rctractors.net
This is the generic code used to control homemade RC Tractors.
The code is intended to be used with an Arduino Pro Mini
Circuit diagram can be found ar www.rctractors.net
Pin Assignment
D0 Serial RX
D1 Serial TX
D2 Left Indicator
D3 Head Lights
D4 Right Indicator
D5 Drive Motor Speed Control
D6 Brake Lights
D7 Drive Motor Direction Control 1
D8 Drive Motor Direction Control 2
D9 Three-Point Linkage Servo Signal
D10 Steering Servo Signal
D11 Accessory Servo Signal
D12 Accessory Serial TX Signal
D13 Spot Lights
A0 Beacon 1
A1 Beacon 2
A2 Not assigned
A3 Not assigned
A4 Not assigned
A5 Not assigned
*/
// This section describes the vehicle
byte ID1 = 0x4A; // Enter a four byte ID for the vehicle
byte ID2 = 0x44; // for example F936 for the Fendt 936 model
byte ID3 = 0x38; // would be ID1 = 0x46; ID2 = 0x39; ID3 = 0x33; ID4 = 0x36
byte ID4 = 0x33; // because asci character F is 46 in hexidecimal and so on
byte steering_center = 0x52; // Every model is different so once we have biult the model
byte steering_min = 0x40; // we test for the limits of the steering. Servo values must
byte steering_max = 0x80; // not exceed 0xB4 as this is 180 degrees, max servo rotation
byte drive_center = 128; // The drive is generally going to be the same for all models
byte drive_max = 255; // but you can change these values if you need to
byte drive_min = 0;
byte link_center = 0x5A; // The link values depend your use. If used to control one of
byte link_min = 0x40; // the three-point links, adjust the values. If used for an
byte link_max = 0x80; // accessory, dont change values and use like external servo
byte external_center = 0x5A; // Set a center value for the extra servos, 5A is 90 in Hex
byte ex_ser_val = 30;
// Variables used in the program
int left_ind = 0; // Variable indicating if the left indicator is currently on or off
int right_ind = 0; // Variable indicating if the right indicator is currently on or off
int ind_timer = 0; // Variable used to record the length of time the indicator is on for
int ind_timer2 = 0; // Variable used to record the beacon is on for
int beac_on = 0; // Variable indicating if the right indicator is currently on or off
int beac_timer = 0; // Variable used to record the length of time the indicator is on for
int ind_active = 0; // Variable set to show that indicator code should be running
int beacon_active = 0; // Variable set to show that indicator code should be running
int dips_on = 0; // Variable to idicate if the dipped headlights are on
int heads_on = 0; // Variable to idicate if the full beams are on
int spots_on = 0; // Variable to idicate if the large spot lights are on
int move1 = 125; // Variable used to determine the motor direction
int move2 = 125;
byte command_val = 0x00; // Stores the command value
byte data_val = 0x00; // Stores the data value
byte servo_val = 0x00; // Variable used to ensure data_val is between servo limits
byte drive_val = 0x00;
byte dat1 = 0x00; // Stores received byte
byte dat2 = 0x00; // Stores received byte
byte dat3 = 0x00; // Stores received byte
byte dat4 = 0x00; // Stores received byte
byte dat5 = 0x00; // Stores received byte
byte dat6 = 0x00; // Stores received byte
byte steering_val = 0x00;
int link_ser_val = 127;
// Battery Voltage Measurement Code
//long readVcc() {
// long result;
// // Read 1.1V reference against AVcc
// ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
// delay(2); // Wait for Vref to settle
// ADCSRA |= _BV(ADSC); // Convert
// while (bit_is_set(ADCSRA,ADSC));
// result = ADCL;
// result |= ADCH<<8;
// result = 1125300L / result; // Back-calculate AVcc in mV
// return result;
//}
#include <Servo.h> // Include the library for controlling servos
#include <SendOnlySoftwareSerial.h> // Include the library for creating additional serial ports
SendOnlySoftwareSerial ExternalSerial(12); // Set pin 2 as RX and 7 as TX for the software serial port
Servo link_servo; // Create servo object to control the link servo
Servo external_servo; // Create servo object to control the external servo
int data = 0; // Variable to indicate serial data is available
void setup()
{
Serial.begin(9600); // Initialize main serial communication
ExternalSerial.begin(9600); // Initialize additional serial communication
// Set pins as outputs
pinMode(2, OUTPUT); // Left indicator
pinMode(3, OUTPUT); // Head lights
pinMode(4, OUTPUT); // Right indicator
pinMode(5, OUTPUT); // Drive motor 1 PWM control pin
pinMode(6, OUTPUT); // Drive motor 2 PWM control pin
pinMode(7, OUTPUT); // Drive motor 1 direction control 1
pinMode(8, OUTPUT); // Drive motor 1 direction control 2
pinMode(10, OUTPUT); // Brake Lights
pinMode(13, OUTPUT); // Spot Lights
pinMode(A0, OUTPUT); // Drive motor 2 direction control 1
pinMode(A1, OUTPUT); // Drive motor 2 direction control 2
pinMode(A2, OUTPUT); // Not assigned
pinMode(A3, OUTPUT); // Not assigned
pinMode(A4, OUTPUT); // Not assigned
pinMode(A5, OUTPUT); // Not assigned
// Set servo pins
link_servo.attach(9); // attaches the servo on pin 9 to the link servo object
external_servo.attach(11); // attaches the servo on pin 11 to the external servo object
// Initialise the servos
link_servo.write(link_center); // sets the initial link servo position
external_servo.write(external_center); // sets the initial external servo position
}
// Main loop, this constantly repeats
void loop() {
if (Serial.available()) { // Check if serial data is available
dat1=Serial.read(); // If yes then read it
if (dat1 == 0x0A){ // Check for 0A indicating the start of a command string
data++; // If 0A recieved then make data = 1
while (data == 1){ // While data = 1 wait for the first command byte
if (Serial.available()) {
command_val=Serial.read(); // Once receieved store it in dat1
data++; // Increment data variable
}
}
while (data == 2){ // While data = 2 wait for the second command byte
if (Serial.available()) {
data_val=Serial.read(); // Once receieved store it in dat2
data = 0; // When the final byte is recieved we exit these loops
}
}
}
}
switch (command_val){ // Perform an action based on the recieved command
case 0x00:
{ // No command exit command selection
break;
}
case 0x01: // Drive command
{
drive_val = data_val;//drive_min, drive_max); // scale the data value for use with the motor
update_motor();
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x02: // Steering command
{
steering_val = data_val; // Scale data_val to between steering_min and steering_max
update_motor();
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x03: // External Servo
{ // We will scale this value with the controller so it works for different implements
if(data_val > 0xCC){// && ex_ser_val<255)
ex_ser_val = ex_ser_val +1;
ExternalSerial.write(0x09);
}
else if(data_val < 0x40){// && ex_ser_val>0)
ex_ser_val = ex_ser_val -1;
ExternalSerial.write(0x08);
}
else{
ExternalSerial.write(0x0A);
}
external_servo.write(ex_ser_val); // Set the servo position according to the recieved value
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x04: // External Serial
{
ExternalSerial.write(data_val); // Output serial command to the implement
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x05: // Dipped lights
{
if (dips_on == 0) // If dips are off turn on
{
analogWrite(3, 125); // Set headlight pwm to 50%
analogWrite(6, 125); // Set taillight pwm to 50%
dips_on = 1;
}
else // Or if dips are on turn off
{
analogWrite(3, 0); // Set headlight pwm to 50%
analogWrite(10, 0); // Set taillight pwm to 50%
dips_on = 0;
heads_on = 0;
}
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x06: // Full beams
{
if (heads_on == 0)
{
analogWrite(3, 255); // Set headlight pwm to 100%
analogWrite(10, 125); // Set taillight pwm to 50%
heads_on = 1;
}
else
{
analogWrite(3, 0); // Set headlight pwm to 100%
analogWrite(10, 0); // Set taillight pwm to 50%
dips_on = 0;
heads_on = 0;
}
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x07: // Spot lights
{
if (data_val == 1)
{
digitalWrite(13, HIGH);
}
else
{
digitalWrite(13, LOW);
}
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x08: // Left indicator
{
if (data_val == 1){ // If off turn on
digitalWrite(2, HIGH);
digitalWrite(4, LOW);
}
else if(data_val == 2){
digitalWrite(2, LOW);
digitalWrite(4, HIGH);
}
else if(data_val == 3){
digitalWrite(2, HIGH);
digitalWrite(4, HIGH);
}
else{
digitalWrite(2, LOW);
digitalWrite(4, LOW);
}
}
case 0x0B: // Not Implemented
{
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x0C: // Link servo
{
link_servo.write(data_val); // Set the servo position according to the scaled value
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x0D: // Not Implemented
{
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x0E: // Not Implemented
{
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
case 0x0F: // Not Implemented
{
command_val = 0x00; // Clear command and data values
data_val = 0x00;
break;
}
}
}
void update_motor(){
if (drive_val < 120){ // These if functions leave a buffer zone of 15 bits above and below the drive center value
move1 = map(drive_val, 120, 0, 0,255);//drive_min, drive_max); // scale the data value for use with the motor
analogWrite(5,move1); // Set the PWM vlue on pin 5 to move the motor
digitalWrite(8, HIGH); // Set the direction with pins 7 and 8
digitalWrite(7, LOW);
}
else if (drive_val > 135){
move1 = map(drive_val, 135, 255, 0,255);//drive_min, drive_max); // scale the data value for use with the motor
analogWrite(5,move1); // Set the PWM vlue on pin 5 to move the motor
digitalWrite(7, HIGH); // Set the direction with pins 7 and 8
digitalWrite(8, LOW);
}
else{
move1=0;
analogWrite(5,move1); // Set the PWM vlue on pin 5 to move the motor
digitalWrite(8, LOW); // Set the direction with pins 7 and 8
digitalWrite(7, LOW);
}
if(steering_val < 120){
move2 = map(steering_val, 128, 0, 0,255);//drive_min, drive_max); // scale the data value for use with the motor
analogWrite(6,move2); // Set the PWM vlue on pin 5 to move the motor
digitalWrite(A1, LOW); // Set the direction with pins 7 and 8
digitalWrite(A0, HIGH);
}
else if(steering_val > 135){
move2 = map(steering_val, 128, 255, 0,255);//drive_min, drive_max); // scale the data value for use with the motor
analogWrite(6,move2); // Set the PWM vlue on pin 5 to move the motor
digitalWrite(A0, LOW); // Set the direction with pins 7 and 8
digitalWrite(A1, HIGH);
}
else{
move2=0;
analogWrite(6,move2); // Set the PWM vlue on pin 5 to move the motor
digitalWrite(A0, LOW); // Set the direction with pins 7 and 8
digitalWrite(A1, LOW);
}
}
void Battery_Check(){
//Batt_Vol = readVcc();
//if(Batt_Vol<3600){
//shutdown
//}
}
There is a long way to go with this code but I'm getting there.