////////////////////////////////////////////////////////////////////////////////
// Name: RO-048 //
// Motordriver Shield V1 test //
// Platform: Arduino Mega 2560 //
// Created by: HARB rboek2@gmail.com June 2016 GPL copyrights //
// http://robotigs.com/robotigs/includes/parts_header.php?idpart=55 //
// This program will drive the Bolderbot Mini, Serial monitor control. //
// Based on Simple Motor Shield sketch by arduino.cc user "Krodal" June 2012 //
////////////////////////////////////////////////////////////////////////////////
// SET PRECOMPILER OPTIONS *****************************************************
// Initialse conditional compiling, uncomment to include, comment to exclude ---
// Do comment for runtime versions
#define RS232 //Uncomment to include Serial Monitor sections
// #ifdef RS232 //Only include these lines if the variable has been defined
// Define precompiler settings SET PINS ----------------------------------------
#define MOTORLATCH 12 //Arduino pins for the shift register of the enigines
#define MOTORCLK 4
#define MOTORENABLE 7
#define MOTORDATA 8
#define MOTOR1_A 3 //These are used to set the direction of the bridge driver
#define MOTOR1_B 2 //The pins are not Arduino pins, but pins on the motorshield
#define MOTOR2_A 1 //Forward and backward settings can be changed here
#define MOTOR2_B 4 //8-bit bus after the 74HC595 shift register
#define MOTOR1_PWM 11 //Arduino pins for the PWM signals for motor 1 left
#define MOTOR2_PWM 3 //Arduino pins for the PWM signals for motor 2 right
#define FORWARD 1 //Translation table for the motor functions
#define BACKWARD 2
#define BRAKE 3
#define RELEASE 4
// Define the needed header files for the precompiler, no charge if not used ---
// Define variables ------------------------------------------------------------
bool ledBinVal = LOW; //You can chose HIGH-on or LOW-off for LED_BUILTIN
uint8_t cmdEngines = 'D'; //The startup colour and the command to the engines
uint8_t inByte = 0; //For incoming serial data
//END OF PRECOMPILER OPTIONS ---------------------------------------------------
void setup() { //Setup runs once ***********************************************
disable_jtag(); //Disable jtag to free port C, enabled by default
#ifdef RS232 //Only include this part if the variable has been defined
Serial.begin(9600); //Nothing more needed for the Serial Monitor to function
#endif //End of Serial not compiled if precompiler variable was not set
pinMode(LED_BUILTIN, OUTPUT); //Arduino boards contain an onboard LED_BUILTIN
toggle_ledBin(); //Show we are awake
}//--(end setup )---------------------------------------------------------------
void loop() { //KEEP ON RUNNING THIS LOOP FOREVER ******************************
#ifdef RS232 //Only include this part if the variable has been defined
readSerialInput(); //Receives and processes data from the Serial Monitor
#endif //End of Serial not compiled if precompiler variable was not set
writeActuators(); //Performs the robot actions according to the inputs
delay(100); //Needed for IR receiver Wait milliseconds, TIME TO SPARE
toggle_ledBin(); //Toggles the on-board LED on or off to tell we are alive
} //End of void loop() //KEEP ON RUNNING THIS LOOP FOREVER
//345678911234567892123456789312345678941234567895123456789612345678971234567898
void writeActuators(void) { //Performs the robot actions according to the inputs
switch (cmdEngines) { //Translate commands in actions
case 'F' : //Command F means FORWARD
motor(1, FORWARD, 255); //Engages Motornr 1=left engine, Command, Speed
motor(2, FORWARD, 255); //Engages Motornr 2=right engine, Command, Speed
break; //Jump to the end of this switch
case 'R' : //Command R means turn RIGHT
motor(1, FORWARD, 255); //Engages Motornr 1=left engine, Command, Speed
motor(2, BACKWARD, 255); //Engages Motornr 2=right engine, Command, Speed
break; //Jump to the end of this switch
case 'L' : //Command L means turn LEFT
motor(1, BACKWARD, 255); //Engages Motornr 1=left engine, Command, Speed
motor(2, FORWARD, 255); //Engages Motornr 2=right engine, Command, Speed
break; //Jump to the end of this switch
case 'B' : //Command B means BACKWARD
motor(1, BACKWARD, 255); //Engages Motornr 1=left engine, Command, Speed
motor(2, BACKWARD, 255); //Engages Motornr 2=right engine, Command, Speed
break; //Jump to the end of this switch
case 'D' : //Command D means DISENGAGE
motor(1, RELEASE, 255); //Engages Motornr 1=left engine, Command, Speed
motor(2, RELEASE, 255); //Engages Motornr 2=right engine, Command, Speed
break; //Jump to the end of this switch
case '0' : //Command 0 means BREAK1
motor(1, BRAKE, 255); //Engages Motornr 1=left engine, Command, Speed
motor(2, BRAKE, 255); //Engages Motornr 2=right engine, Command, Speed
break; //Jump to the end of this switch
default: //If no case matched, then this default case will be performed
break; //Jump to the end of this switch
} //End of switch (cmdEngines) Translate commands in actions DONE ------------
} //End of SUBsetActuators Performs the robot actions according to the inputs
void motor(int nMotor, int command, int speed) { //Main command for engines ****
int motorA, motorB; //The commands are: FORWARD, BACKWARD, BRAKE, RELEASE
if (nMotor >= 1 && nMotor <= 4) { //Select the motor 1-4, see motorshield
switch (nMotor) { //Couple the desired engine to this software
case 1: //If engine = 1 = left motor
motorA = MOTOR1_A;
motorB = MOTOR1_B;
break;
case 2:
motorA = MOTOR2_A;
motorB = MOTOR2_B;
break;
default:
break;
}
switch (command) {
case FORWARD:
motor_output (motorA, HIGH, speed);
motor_output (motorB, LOW, -1); // -1: no PWM set
break;
case BACKWARD:
motor_output (motorA, LOW, speed);
motor_output (motorB, HIGH, -1); // -1: no PWM set
break;
case BRAKE:
// The AdaFruit library didn't implement a brake.
// The L293D motor driver ic doesn't have a good
// brake anyway.
// It uses transistors inside, and not mosfets.
// Some use a software break, by using a short
// reverse voltage.
// This brake will try to brake, by enabling
// the output and by pulling both outputs to ground.
// But it isn't a good break.
motor_output (motorA, LOW, 255); // 255: fully on.
motor_output (motorB, LOW, -1); // -1: no PWM set
break;
case RELEASE:
motor_output (motorA, LOW, 0); // 0: output floating.
motor_output (motorB, LOW, -1); // -1: no PWM set
break;
default:
break;
}
}
}
// ---------------------------------
// motor_output
//
// The function motor_ouput uses the motor driver to
// drive normal outputs like lights, relays, solenoids,
// DC motors (but not in reverse).
//
// It is also used as an internal helper function
// for the motor() function.
//
// The high_low variable should be set 'HIGH'
// to drive lights, etc.
// It can be set 'LOW', to switch it off,
// but also a 'speed' of 0 will switch it off.
//
// The 'speed' sets the PWM for 0...255, and is for
// both pins of the motor output.
// For example, if motor 3 side 'A' is used to for a
// dimmed light at 50% (speed is 128), also the
// motor 3 side 'B' output will be dimmed for 50%.
// Set to 0 for completelty off (high impedance).
// Set to 255 for fully on.
// Special settings for the PWM speed:
// Set to -1 for not setting the PWM at all.
//
void motor_output (int output, int high_low, int speed)
{
int motorPWM;
switch (output)
{
case MOTOR1_A:
case MOTOR1_B:
motorPWM = MOTOR1_PWM;
break;
case MOTOR2_A:
case MOTOR2_B:
motorPWM = MOTOR2_PWM;
break;
default:
// Use speed as error flag, -3333 = invalid output.
speed = -3333;
break;
}
if (speed != -3333)
{
// Set the direction with the shift register
// on the MotorShield, even if the speed = -1.
// In that case the direction will be set, but
// not the PWM.
shiftWrite(output, high_low);
// set PWM only if it is valid
if (speed >= 0 && speed <= 255)
{
analogWrite(motorPWM, speed);
}
}
}
// ---------------------------------
// shiftWrite
//
// The parameters are just like digitalWrite().
//
// The output is the pin 0...7 (the pin behind
// the shift register).
// The second parameter is HIGH or LOW.
//
// There is no initialization function.
// Initialization is automatically done at the first
// time it is used.
//
void shiftWrite(int output, int high_low)
{
static int latch_copy;
static int shift_register_initialized = false;
// Do the initialization on the fly,
// at the first time it is used.
if (!shift_register_initialized)
{
// Set pins for shift register to output
pinMode(MOTORLATCH, OUTPUT);
pinMode(MOTORENABLE, OUTPUT);
pinMode(MOTORDATA, OUTPUT);
pinMode(MOTORCLK, OUTPUT);
// Set pins for shift register to default value (low);
digitalWrite(MOTORDATA, LOW);
digitalWrite(MOTORLATCH, LOW);
digitalWrite(MOTORCLK, LOW);
// Enable the shift register, set Enable pin Low.
digitalWrite(MOTORENABLE, LOW);
// start with all outputs (of the shift register) low
latch_copy = 0;
shift_register_initialized = true;
}
// The defines HIGH and LOW are 1 and 0.
// So this is valid.
bitWrite(latch_copy, output, high_low);
// Use the default Arduino 'shiftOut()' function to
// shift the bits with the MOTORCLK as clock pulse.
// The 74HC595 shiftregister wants the MSB first.
// After that, generate a latch pulse with MOTORLATCH.
shiftOut(MOTORDATA, MOTORCLK, MSBFIRST, latch_copy);
delayMicroseconds(5); // For safety, not really needed.
digitalWrite(MOTORLATCH, HIGH);
delayMicroseconds(5); // For safety, not really needed.
digitalWrite(MOTORLATCH, LOW);
}
void readSerialInput(void) { //Receiv and proces data from the Serial Monitor
#ifdef RS232 //Only include this part if the variable has been defined
if (Serial.available() > 0) { //Send data only when you receive data
inByte = Serial.read(); //Read the incoming byte
Serial.print("The keyboard just said: "); //Say what you got
Serial.println(inByte, DEC); //Possible translated in DEC HEX OCT BIN
if (inByte == 100) { //d to disengage both engines
cmdEngines = 'D';
}
if (inByte == 104) { //h to disengage both engines
cmdEngines = 'D';
}
if (inByte == 49) { //1 for Autorun1
cmdEngines = '1';
}
if (inByte == 102) { //f for forward
cmdEngines = 'F';
}
if (inByte == 114) { //r for right turn
cmdEngines = 'R';
}
if (inByte == 108) { //l for left turn
cmdEngines = 'L';
}
if (inByte == 98) { //b for backward
cmdEngines = 'B';
}
if (inByte == 32) { //spacebar for break1
cmdEngines = '0';
}
if (inByte == 112) { //p for panic
cmdEngines = 'P';
}
toggle_ledBin(); //Toggles the LED_BUILTIN on-board LED on or off
}//End of if (Serial.available() > 0
#endif //End of Serial not compiled if precompiler variable was not set
} //End of readSerialInput Receives and processes data from the Serial Monitor
void disable_jtag(void) { //Disable jtag to free port C, enabled by default ****
#if defined(JTD) //Not all AVR controller include jtag
MCUCR |= ( 1 << JTD ); //Write twice to disable
MCUCR |= ( 1 << JTD ); //So stutter once
#endif //End of conditional compiling
} //Exit jtag_disable ----------------------------------------------------------
void toggle_ledBin(void) { //Toggles the LED_BUILTIN on-board LED on or off ****
ledBinVal = !ledBinVal; //Toggle value
digitalWrite(LED_BUILTIN, ledBinVal); //Set Arduino boards onboard LED
} //Exit toggle_ledBin ---------------------------------------------------------
////////////////////////////////////////////////////////////////////////////////
// FUSES (can always be altered by using the STK500) //
// On-Chip Debug Enabled: off (OCDEN=0) //
// JTAG Interface Enabled: off (JTAGEN=0) //
// Preserve EEPROM mem through the Chip Erase cycle: On (EESAVE = 0) //
// Boot Flash section = 2048 words, Boot startaddr=$3800 (BOOTSZ=00) //
// Boot Reset vector Enabled, default address=$0000 (BOOTSTR=0) //
// CKOPT fuse (operation dependent of CKSEL fuses (CKOPT=0) //
// Brown-out detection level at VCC=2,7V; (BODLEVEL=1) //
// Ext. Cr/Res High Freq.; Start-up time: 16K CK + 64 ms (CKSEL=1111 SUT=11) //
// LOCKBITS (are dangerous to change, since they cannot be reset) //
// Mode 1: No memory lock features enabled //
// Application Protect Mode 1: No lock on SPM and LPM in Application Section //
// Boot Loader Protect Mode 1: No lock on SPM and LPM in Boot Loader Section //
////////////////////////////////////////////////////////////////////////////////
// EEPROM MEMORY MAP: //
// Start End Number Description //
// 0000 0000 1 Never use this memory location to be AVR compatible //
////////////////////////////////////////////////////////////////////////////////
// PIN ALLOCATIONS //
// Prog=List=Chip //
// A0 = 97 = PF0 ADC0 = ADC //
// A1 = 96 = PF1 ADC1 = ADC //
// A2 = 95 = PF2 ADC2 = ADC //
// A3 = 94 = PF3 ADC3 = ADC //
// A4 = 93 = PF4 ADC4/TMK = ADC //
// A14 = 83 = PK6 ADC14/PCINT22 = Battery monitor H=1/2 6Vdc pack ADC //
// A15 = 82 = PK7 ADC15/PCINT23 = Battery monitor H=1/3 9Vdc battery ADC //
// 0 = 2 = PE0 RXD0/PCINT8 = Serial monitor, also on-board LED RX0 //
// 1 = 3 = PE1 TXD0 = Serial monitor, also on-board LED TX0 //
// 2 = 6 = PE4 OC3B/INT4 = IR TV remote control receiver INT //
// 3 = 7 = PE5 OC3C/INT5 = Motorshield INT //
// 4 = 1 = PG5 OCOB = Motorshield PWM //
// 5 = 5 = PE3 OC3A/AIN1 = Motorshield PWM //
// 6 = 15 = PH3 OC4A = Motorshield PWM //
// 7 = 16 = PH4 OC4B = Motorshield PWM //
// 8 = 17 = PH5 OC4C = Motorshield PWM //
// 9 = 18 = PH6 OC2B = Motorshield PWM //
// 10 = 23 = PB4 OC2A/PCINT4 = Motorshield PWM //
// 11 = 24 = PB5 OC1A/PCINT5 = Motorshield PWM //
// 12 = 25 = PB6 OC1B/PCINT6 = Motorshield PWM //
// 13 = 26 = PB7 OCOA/OC1C/PCINT7 = On board user LED, on=high off=low PWM //
// 18 = 46 = PD2 TXD1/INT3 = Speed encoder Right TX1 //
// 19 = 45 = PD2 RXD1/INT2 = Speed encoder Left RX1 //
// 20 = 44 = PD1 SDA/INT1 = SDA Yellow SRF10 TWI //
// 21 = 43 = PD0 SCL/INT0 = SCL White SRF10 TWI //
// 44 = 40 = PL5 OC5C = 3 Color led Blue PWM //
// 45 = 39 = PL4 OC5B = 3 Color led Red PWM //
// 46 = 38 = PL3 OC5A = 3 Color led Green PWM //
// 50 = 22 = PB3 MISO/PCINT3 = SPI //
// 51 = 21 = PB2 MOSI/PCINT2 = SPI //
// 52 = 20 = PB1 SCK/PCINT1 = SPI //
// 53 = 19 = PB1 SS/PCINT0 = SPI //
////////////////////////////////////////////////////////////////////////////////
//345678911234567892123456789312345678941234567895123456789612345678971234567898