Search
Forum
Wiki
Images
Files
Member List
Help
I'll go ahead and paste the code, 'cause... http://rebeldroids.net/gallery/displayim...1&pid=1432
Code:
//
// HO15 Brain Program
//
// On an Arduino Leonardo or ...
// Nano, etc would need Serial1 changed to Serial
//
#include "FastLED.h"
// Hardware serial is used to multiplex motor controllers as we have
// lots of motors on L115. Most communication will be the "pololu
// protocol" so that we can select which controller to ask.
//
// Motors are on Serial1 (Arduino Leonardo)
// LEDs are on Pin 6
//
// If needed, we'll use software serial for other devices.
//
// First & Second motor controllers contain R/C inputs
//
// First motor controller is queried for battery voltage for failsafe
//
// RX will get power from controller 1
// Motors
#define FRONT_LEFT 13
#define FRONT_RIGHT 14
#define REAR_RIGHT 15
#define REAR_LEFT 16
#define MOTOR_FIRST FRONT_LEFT
#define MOTOR_LAST REAR_LEFT
#define MOTOR_BAUD 38400
int motorSpeeds[4];
#define SPEED_FRONT_LEFT 0
#define SPEED_FRONT_RIGHT 1
#define SPEED_REAR_RIGHT 2
#define SPEED_REAR_LEFT 3
// R/CInputs
// Values will be scaled -3200 to 3200
// Rotate is Channel 1 of Controller 1
#define CHANNEL_ROTATE 6
#define CONTROLLER_ROTATE FRONT_LEFT
// Forward/Backward is Channel 1 of Controller 2
#define CHANNEL_MOVE 6
#define CONTROLLER_MOVE REAR_LEFT
// Sideways is Channel 2 of Controller 2
#define CHANNEL_SIDEWAYS 10
#define CONTROLLER_SIDEWAYS REAR_LEFT
int stickRotate = 0;
int stickMove = 0;
int stickSideways = 0;
// Alert for low volts (mV)
#define LOW_VOLT_CONTROLLER 1
int lowVoltCheck = 12800; // 12750?
bool lowVoltAlert = false;
// Want to check volts periodically (not all the time so it doesn't slow us down)
unsigned long nextVoltCheck = 0;
// Check every 60 seconds
#define voltCheckPeriod = 60000
// LEDs are needed for L115's Neck
// #0 - Front Blue Center (first one on string)
// #1 - Left Center Red (blinks orange?)
// #2 - Rear Left Orange
// #3 - Right Rear Binks red
// #4 - Right Green
// Number of RGB LEDs
#define NUM_LEDS 5
// LED Pin was 3?
#define LEDPIN 6
// Define the array of leds
CRGB leds[NUM_LEDS];
#define LED_FRONT 0
#define LED_LEFT 1
#define LED_REAR 2
#define LED_RIGHT_A 3
#define LED_RIGHT_B 4
// Toggle spewing of debug spew
bool debug = true;
// MP3 Trigger code not in this version.
// the setup routine runs once when you press reset:
void setup()
{
Serial1.begin(MOTOR_BAUD); // Motor Controller
// Not really needed, but tell motor controller(s) to autodetect baud rate
Serial1.write(0xAA);
if (debug)
{
Serial.begin(38400);
Serial.write("hi there");
}
// Visual indicator for debugging
pinMode(LED_BUILTIN, OUTPUT);
// Make sure all motors stopped, then configure for running
stopMotors();
setBrake();
startMotors();
if (debug)
{
// Quick test program
// THIS MAKES THE MOTORS MOVE, COMMENT OUT TO NOT DO THIS!!!
// testMotors();
}
// LEDs
// WS2811 have different color patterns than NEOPIXEL/WS2812B
// FastLED.addLeds<NEOPIXEL, NEOPIXELPIN>(leds, NUM_LEDS);
FastLED.addLeds<WS2811, LEDPIN>(leds, NUM_LEDS);
if (debug)
{
// Do the LED test sequence
for (int i = 0; i < 1; i++)
{
testLEDs();
}
}
// Set them back to normal
setNormalLEDs();
FastLED.show();
}
// the loop routine runs over and over again forever:
void loop()
{
// Do motors
readPositions();
calculateSpeeds();
setMotorSpeeds();
// Do LEDs
updateLEDs();
}
void testLEDs()
{
for (int i = 0; i < NUM_LEDS; i++)
{
leds[i].r = 255;
FastLED.show();
delay(250);
leds[i].g = 255;
leds[i].r = 0;
FastLED.show();
delay(250);
leds[i].g = 0;
leds[i].b = 255;
FastLED.show();
delay(250);
leds[i].b = 0;
}
}
// We're updating 4x/second
void updateLEDs()
{
// Our things happen 4x/second
int time = (millis() % 1000) / 250;
// Right side counts down from 3 in binary
// Right Red is 2's on time 0 & 1
leds[LED_RIGHT_A].r = 0;
leds[LED_RIGHT_A].g = 0;
leds[LED_RIGHT_A].b = 0;
if (time < 2)
{
leds[LED_RIGHT_A].r = 255;
}
// Right Green is 1's on time 0 & 2
leds[LED_RIGHT_B].r = 0;
leds[LED_RIGHT_B].g = 0;
leds[LED_RIGHT_B].b = 0;
if (time == 0 || time == 2)
{
leds[LED_RIGHT_B].g = 255;
}
// Left & rear LED blinks red about 2hz, but slightly off of right side?
time = (millis() % 510) / 255;
leds[LED_REAR].r = 0;
leds[LED_REAR].g = 0;
leds[LED_REAR].b = 0;
leds[LED_LEFT].r = 0;
leds[LED_LEFT].g = 0;
leds[LED_LEFT].b = 0;
if (time == 0)
{
leds[LED_REAR].r = 255;
leds[LED_LEFT].r = 255;
}
// Front is Blue
leds[LED_FRONT].r = 0;
leds[LED_FRONT].g = 0;
leds[LED_FRONT].b = 255;
// Show our updates
FastLED.show();
}
// Static for no good reason
void setNormalLEDs()
{
// Front is Blue
leds[LED_FRONT].r = 0;
leds[LED_FRONT].g = 0;
leds[LED_FRONT].b = 255;
// Left is Red (blinks)
leds[LED_LEFT].r = 255;
leds[LED_LEFT].g = 0;
leds[LED_LEFT].b = 0;
// Rear is Red (one LED, leftish)
leds[LED_REAR].r = 255;
leds[LED_REAR].g = 0;
leds[LED_REAR].b = 0;
// Right (rear) is red (blinks)
leds[LED_RIGHT_A].r = 255;
leds[LED_RIGHT_A].g = 0;
leds[LED_RIGHT_A].b = 0;
// Right (more middle) is green (blinks)
leds[LED_RIGHT_B].r = 0;
leds[LED_RIGHT_B].g = 255;
leds[LED_RIGHT_B].b = 0;
FastLED.show();
}
void startMotors()
{
// exit safe stop
for (byte motor = MOTOR_FIRST; motor <= MOTOR_LAST; motor++)
{
Serial1.write(0xAA);
Serial1.write(motor);
Serial1.write(0x3); // Start
}
}
void stopMotors()
{
// Stop motors
for (byte motor = MOTOR_FIRST; motor <= MOTOR_LAST; motor++)
{
Serial1.write(0xAA);
Serial1.write(motor);
Serial1.write(0x60); // Stop motor
motorSpeeds[motor-MOTOR_FIRST] = 0;
}
}
void setBrake()
{
for (byte motor = MOTOR_FIRST; motor <= MOTOR_LAST; motor++)
{
Serial1.write(0xAA);
Serial1.write(motor);
Serial1.write(0x12); // Set brake
Serial1.write(10); // 0-32
}
}
void setMotorSpeeds()
{
for (byte motor = 0; motor <= 3; motor++)
{
int thisSpeed = motorSpeeds[motor];
if (debug)
{
Serial.print("motor ");
Serial.print(motor);
Serial.print(":");
Serial.println(thisSpeed);
}
Serial1.write(0xAA);
Serial1.write(motor + MOTOR_FIRST);
// Forward or backward command?
if (thisSpeed < 0)
{
// Backwards
Serial1.write(0x6); // Motor Reverse
thisSpeed = -thisSpeed;
}
else
{
// Forwards
Serial1.write(0x5); // Motor Forward
}
if (thisSpeed > 3200)
{
thisSpeed = 3200;
}
Serial1.write(thisSpeed&0x1f);
Serial1.write(thisSpeed>>5);
}
}
void readPositions()
{
// We shouldn't have anything available.
while(Serial1.available())
{
if (debug) Serial.println("out-of-sync!!!");
Serial1.read();
}
// Rotation
Serial1.write(0xAA);
Serial1.write(CONTROLLER_ROTATE);
Serial1.write(0x21); // Get Variable
Serial1.write(CHANNEL_ROTATE); // R/C 1 Scaled variable
delay(1);
Serial1.write(0xAA);
Serial1.write(CONTROLLER_MOVE);
Serial1.write(0x21); // Get Variable
Serial1.write(CHANNEL_MOVE); // R/C 1 Scaled variable
delay(1);
Serial1.write(0xAA);
Serial1.write(CONTROLLER_SIDEWAYS);
Serial1.write(0x21); // Get Variable
Serial1.write(CHANNEL_SIDEWAYS);// R/C 1 Scaled variable
delay(2);
// Hopefully these all got queue'd up in order
// Each value should be -3200 to 3200
if (Serial1.available() < 2)
{
if (debug) Serial.println("delay1");
delay(1);
}
stickRotate = Serial1.read() + Serial1.read()*256;
if (Serial1.available() < 2)
{
if (debug) Serial.println("delay2");
delay(1);
}
stickMove = Serial1.read() + Serial1.read()*256;
if (Serial1.available() < 2)
{
if (debug) Serial.println("delay3");
delay(1);
}
stickSideways = Serial1.read() + Serial1.read()*256;
if (debug)
{
Serial.print("rotate: ");
Serial.println(stickRotate);
Serial.print("move: ");
Serial.println(stickMove);
Serial.print("sideways: ");
Serial.println(stickSideways);
// Also blink the onboard LED in case computer isn't hooked up.
// Change these two "stickRotate" checks to
// "stickMove" or "stickSideways" to test other R/C inputs.
if (stickRotate < -100)
{
digitalWrite(LED_BUILTIN, LOW);
}
if (stickRotate > 100)
{
digitalWrite(LED_BUILTIN, HIGH);
}
}
}
void calculateSpeeds()
{
// Everyone gets the rotate speed
// left gets move
// right gets -move
// front gets sideways
// rear gets - sideways
motorSpeeds[SPEED_FRONT_LEFT] = stickRotate + stickMove + stickSideways;
motorSpeeds[SPEED_FRONT_RIGHT] = stickRotate - stickMove + stickSideways;
motorSpeeds[SPEED_REAR_RIGHT] = stickRotate - stickMove - stickSideways;
motorSpeeds[SPEED_REAR_LEFT] = stickRotate + stickMove - stickSideways;
}
// Test the motors
void testMotors()
{
for (int count = 0; count < 3; count++)
{
for (int i = 0; i < 3; i++)
{
// speed is -3200 to 3200, so fast enough to see it's doing something, not so fast as to go crazy
motorSpeeds[i] = 600;
setMotorSpeeds();
delay(250);
motorSpeeds[i] = 0;
setMotorSpeeds();
}
for (int i = 0; i < 3; i++)
{
// speed is -3200 to 3200, so fast enough to see it's doing something, not so fast as to go crazy
motorSpeeds[i] = -600; // Try the other direction
setMotorSpeeds();
delay(250);
motorSpeeds[i] = 0;
setMotorSpeeds();
}
}
// Make sure they're all off.
stopMotors();
startMotors();
}