void driveRobotActual(void) {
byteTx(CmdDriveWheels);
byteTx(rightVeolcityHigh);
byteTx(rightVelocityLow);
byteTx(leftVelocityHigh);
byteTx(leftVelocityLow);
}
/** Turn the robot angle theta
* theta in radians (theta * 1000)
* positive angles indicate counter-clockwise turns
* negative angles indicate clockwise turns
*/
void turn(int theta) {
uint8_t rHi;
uint8_t rLo;
uint8_t lHi;
uint8_t lLo;
if (theta > 0) {
rHi = vHiPos;
rLo = vLoPos;
lHi = vHiNeg;
lLo = vLoNeg;
} else {
rHi = vHiNeg;
rLo = vLoNeg;
lHi = vHiPos;
lLo = vLoPos;
theta *= -1;
}
byteTx(CmdDriveWheels);
byteTx(rHi);
byteTx(rLo);
byteTx(lHi);
byteTx(lLo);
delayMs(theta);
stop();
}
int main(void) {
// Disable interrupts. ("Clear interrupt bit")
cli();
// One-time setup operations.
setupSerialPort();
setupRightLED();
setupLeftLED();
setupTimer();
// Enable interrupts. ("Set interrupt bit")
sei();
byteTx(128); // Start the open interface.
byteTx(132); // Switch to full mode.
// Toggle the LEDs once each second.
while(2+2==4) {
rightLEDon();
leftLEDoff();
byteTx(139); // Opcode for "Set LEDs"
byteTx(10); // Led bits: both on
byteTx(0); // Power led color: Fully green
byteTx(255); // Power led intensity
delayMs(1000);
rightLEDoff();
leftLEDon();
byteTx(139); // Opcode for "Set LEDs"
byteTx(0); // Led bits: both off
byteTx(255); // Power led color: Fully red
byteTx(255); // Power led intensity
delayMs(1000);
}
}
void driveRobotImmediateStop(void) {
byteTx(CmdDriveWheels);
byteTx(0);
byteTx(0);
byteTx(0);
byteTx(0);
}
//Turn on power Led given a specified color.
void powerLed(uint8_t color) {
byteTx(CmdLeds);
byteTx(0x00); //both command module Leds off
byteTx(color);
byteTx(255); //intensity
}
void drive(int16_t velocity, int16_t radius) {
// Send the start driving command to the Create
byteTx(CmdDrive);
byteTx((uint8_t)((velocity >> 8) & 0x00FF));
byteTx((uint8_t)(velocity & 0x00FF));
byteTx((uint8_t)((radius >> 8) & 0x00FF));
byteTx((uint8_t)(radius & 0x00FF));
}
// Send Create patrol_room commands in terms of velocity and radius
void patrol_room(int16_t velocity, int16_t radius)
{
byteTx(CmdDrive);
byteTx((uint8_t)((velocity >> 8) & 0x00FF));
byteTx((uint8_t)(velocity & 0x00FF));
byteTx((uint8_t)((radius >> 8) & 0x00FF));
byteTx((uint8_t)(radius & 0x00FF));
}
void driveDistanceOp(int16_t velocity, int16_t distance) {
// Start driving
drive(velocity, RadStraight);
// Halt execution of new commands on the Create until reached distance
byteTx(WaitForDistance);
byteTx((uint8_t)((distance >> 8) & 0x00FF));
byteTx((uint8_t)(distance & 0x00FF));
// Stop the Create
driveStop();
}
// Declare Global variables
int main(void) {
// Set up Create and module
initializeCommandModule();
powerOnRobot();
// Is the Robot on
byteTx(CmdStart);
// Start the create
baud(Baud57600);
// Set the baud rate for the Create and Command Module
defineSongs();
// Define some songs so that we know the robot is on.
byteTx(CmdControl);
// Deprecated form of safe mode. I use it because it will
// turn of all LEDs, so it's essentially a reset.
byteTx(CmdFull);
// We are operating in FULL mode.
// Play the reset song and wait while it plays.
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayMs(750);
// Turn the power button on to something.
turnOnPowerButtonLight();
delayMs(20);
// Infinite operation loop
int timerLoop = 0;
const int timerLimit = 15;
initializeUSBBuffer();
initializeRobotBuffer();
initializeSensorArray();
setLEDs(BOTHLED, FULL);
enableReports();
enableSensors();
while(1) {
pollSensors();
if(timerLoop == timerLimit){
compileReport();
sendUSBBuffer();
timerLoop = 0;
delayMs(1);
}
timerLoop++;
pollRemoteSensors();
pollRemote();
executeCommand();
delayMs(5);
}
}
int main(void)
{
//setting up the timer and leds
setup();
//gives the robot time to accept the commands to make squares
delayMs(1000);
//sets the play and advance LEDs off and the power LED to red
robotLED(off, robotPowerRed, robotPowerOn);
while (1)
{
delayMs(100);
// Ask the robot about its bump sensors.
byteTx(opReadSensors); // Opcode for "Read sensors"
byteTx(packetButtons); // Sensor packet 7: Bumps and wheel drops
// Read the one-byte response and extract the relevant bits.
uint8_t buttons = byteRx();
uint8_t playButton = buttons & (1 << 0);
uint8_t advanceButton = buttons & (1 << 2);
//makes a square counterclockwise of legnth 250 mm at velocity 300 mm/s
if(advanceButton)
{
//turn on the advance LED on the robot and sets the power LED to
delayMs(500);
//turns on the advance LED and sets the power LED to green
robotLED(robotAdvanceLED, robotPowerGreen, robotPowerOn);
//makes a square countercolokwise with sides = 250mm at 300 mm/s
makeSquareCCW(300, 250);
}
//makes a square clockwise of legnth 250 mm at velocity 300 mm/s
if(playButton)
{
//turn on the play LED on the robot and sets the power LED to green
delayMs(500);
//turn on the play LED and sets the power LED to green
robotLED(robotPlayLED, robotPowerGreen, robotPowerOn);
//makes a square colokwise with sides = 250mm at 300 mm/s
makeSquareCW(300, 250);
}
//turns off the play LED and sets the pwer LED to red
robotLED(off, robotPowerRed, robotPowerOn);
}
}
// Declare Global variables
int main(void) {
// Set up Create and module
initializeCommandModule();
powerOnRobot();
// Is the Robot on
byteTx(CmdStart);
// Start the create
baud(Baud57600);
// Set the baud rate for the Create and Command Module
defineSongs();
// Define some songs so that we know the robot is on.
byteTx(CmdControl);
// Deprecated form of safe mode. I use it because it will
// turn of all LEDs, so it's essentially a reset.
byteTx(CmdFull);
// We are operating in FULL mode.
// Play the reset song and wait while it plays.
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayMs(750);
// Turn the power button on to something.
delayMs(20);
initializeUSBBuffer();
initializeRobotBuffer();
initializeSensorArray();
setLEDs(BOTHLED, FULL);
enableReports();
enableSensors();
int fakeTimer = 0;
// Infinite operation loop
while(1) {
if (fakeTimer == 4) {
compileReport();
sendUSBBuffer();
fakeTimer = 0;
}
pollSensors();
delayMs(250);
fakeTimer++;
}
}
void driveAngleOp(int16_t velocity, int16_t radius, int16_t angle) {
// Wait for angle opcode compatibility
if (radius == RadCW) {
angle = -angle;
}
// Start driving
drive(velocity, radius);
// Halt execution of new commands on the Create until reached angle
byteTx(WaitForAngle);
byteTx((uint8_t)((angle >> 8) & 0x00FF));
byteTx((uint8_t)(angle & 0x00FF));
// Stop the Create
driveStop();
}
/** Drive the robot at rate 'rate' until stopped
* rate in mm/s
*/
void drive(int rate) {
//rate to hi/lo
uint8_t hi = 0x00;
uint8_t lo = 0x00;
hi |= rate >> 8;
lo |= rate;
byteTx(CmdDriveWheels);
byteTx(hi);
byteTx(lo);
byteTx(hi);
byteTx(lo);
}
void baud28k(void)
{
// Send the baud change command for 28800 baud
byteTx(CmdBaud);
byteTx(Baud28800);
// Wait while until the command is sent
while(!(UCSR0A & _BV(TXC0))) ;
// Change the atmel's baud rate
UBRR0 = Ubrr28800;
// Wait 100 ms
delay10ms(10);
}
/** Specify velocities of wheels separately
* rate in mm/s
*/
void driveLR(int rateLeft, int rateRight) {
//rates to hi/lo
uint8_t rHi = 0x00, rLo = 0x00, lHi = 0x00, lLo = 0x00;
lHi |= rateLeft >> 8;
lLo |= rateLeft;
rHi |= rateRight >> 8;
rLo |= rateRight;
byteTx(CmdDriveWheels);
byteTx(rHi);
byteTx(rLo);
byteTx(lHi);
byteTx(lLo);
}
void baud(uint8_t baud_code) {
// Switch the baud rate on both Create and module
if(baud_code <= 11)
{
byteTx(CmdBaud);
UCSR0A |= _BV(TXC0);
byteTx(baud_code);
// Wait until transmit is complete
while(!(UCSR0A & _BV(TXC0))) ;
cli();
// Switch the baud rate register
if(baud_code == Baud115200) {
UBRR0 = Ubrr115200;
} else if(baud_code == Baud57600) {
UBRR0 = Ubrr57600;
} else if(baud_code == Baud38400) {
UBRR0 = Ubrr38400;
} else if(baud_code == Baud28800) {
UBRR0 = Ubrr28800;
} else if(baud_code == Baud19200) {
UBRR0 = Ubrr19200;
} else if(baud_code == Baud14400) {
UBRR0 = Ubrr14400;
} else if(baud_code == Baud9600) {
UBRR0 = Ubrr9600;
} else if(baud_code == Baud4800) {
UBRR0 = Ubrr4800;
} else if(baud_code == Baud2400) {
UBRR0 = Ubrr2400;
} else if(baud_code == Baud1200) {
UBRR0 = Ubrr1200;
} else if(baud_code == Baud600) {
UBRR0 = Ubrr600;
} else if(baud_code == Baud300) {
UBRR0 = Ubrr300;
}
sei();
delayMs(100);
}
}
// Delay for the specified time in ms and update sensor values
void delayAndUpdateSensors(uint16_t time_ms)
{
uint8_t temp;
timer_on = 1;
timer_cnt = time_ms;
while(timer_on)
{
if(!sensors_flag)
{
for(temp = 0; temp < Sen6Size; temp++)
sensors[temp] = sensors_in[temp];
// Update running totals of distance and angle
distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);
byteTx(CmdSensors);
byteTx(6);
sensors_index = 0;
sensors_flag = 1;
}
}
}
int main() {
// Disable interrupts. ("Clear interrupt bit")
cli();
// One-time setup operations.
setupSerialPort();
setupRightLED();
setupLeftLED();
setupTimer();
// Enable interrupts. ("Set interrupt bit")
sei();
byteTx(128); // Start the open interface.
byteTx(132); // Switch to full Mode
while(2+2==4) {
delayMs(100);
// Ask the robot about its bump sensors.
byteTx(142); // Opcode for "Read sensors"
byteTx(7); // Sensor packet 7: Bumps and wheel drops
// Read the one-byte response and extract the relevant bits.
uint8_t bumps = byteRx();
uint8_t bumpRight = bumps & (1 << 0);
uint8_t bumpLeft = bumps & (1 << 1);
// Set the command module LEDs based on this sensor data.
if(bumpLeft) {
leftLEDon();
}
else {
leftLEDoff();
}
if(bumpRight) {
rightLEDon();
}
else {
rightLEDoff();
}
}
}
// Define songs to be played later
void defineSongs(void)
{
// Reset song
byteTx(CmdSong);
byteTx(RESET_SONG);
byteTx(4);
byteTx(60);
byteTx(6);
byteTx(72);
byteTx(6);
byteTx(84);
byteTx(6);
byteTx(96);
byteTx(6);
// Start song
byteTx(CmdSong);
byteTx(START_SONG);
byteTx(6);
byteTx(69);
byteTx(18);
byteTx(72);
byteTx(12);
byteTx(74);
byteTx(12);
byteTx(72);
byteTx(12);
byteTx(69);
byteTx(12);
byteTx(77);
byteTx(24);
// Bump song
byteTx(CmdSong);
byteTx(BUMP_SONG);
byteTx(2);
byteTx(74);
byteTx(12);
byteTx(59);
byteTx(24);
// End song
byteTx(CmdSong);
byteTx(END_SONG);
byteTx(5);
byteTx(77);
byteTx(18);
byteTx(74);
byteTx(12);
byteTx(72);
byteTx(12);
byteTx(69);
byteTx(12);
byteTx(65);
byteTx(24);
}
// Define songs to be played later
void defineSongs(void) {
// Reset song
byteTx(CmdSong);
byteTx(RESET_SONG);
byteTx(4);
byteTx(60);
byteTx(6);
byteTx(72);
byteTx(6);
byteTx(84);
byteTx(6);
byteTx(96);
byteTx(6);
// Start song
byteTx(CmdSong);
byteTx(START_SONG);
byteTx(6);
byteTx(69);
byteTx(18);
byteTx(72);
byteTx(12);
byteTx(74);
byteTx(12);
byteTx(72);
byteTx(12);
byteTx(69);
byteTx(12);
byteTx(77);
byteTx(24);
}
int main (void)
{
uint8_t leds_cnt = 99;
uint8_t leds_state = 0;
uint8_t leds_on = 1;
int16_t turn_angle = 0;
uint8_t turn_dir = 1;
uint8_t turning = 0;
uint8_t backing_up = 0;
// Set up Create and module
initialize();
LEDBothOff;
powerOnRobot();
byteTx(CmdStart);
baud(Baud28800);
defineSongs();
byteTx(CmdControl);
byteTx(CmdFull);
// Stop just as a precaution
patrol_room(0, RadStraight);
// Play the reset song and wait while it plays
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayAndUpdateSensors(750);
for(;;)
{
if(++leds_cnt >= 100)
{
leds_cnt = 0;
leds_on = !leds_on;
if(leds_on)
{
byteTx(CmdLeds);
byteTx(LEDsBoth);
byteTx(128);
byteTx(255);
LEDBothOff;
}
else
{
byteTx(CmdLeds);
byteTx(0x00);
byteTx(0);
byteTx(0);
LEDBothOn;
}
}
delayAndUpdateSensors(10);
if(UserButtonPressed)
{
// Play start song and wait
byteTx(CmdPlay);
byteTx(START_SONG);
delayAndUpdateSensors(2813);
// Drive around until a button or unsafe condition is detected
while(!(UserButtonPressed)
&& (!sensors[SenCliffL])
&& (!sensors[SenCliffFL])
&& (!sensors[SenCliffFR])
&& (!sensors[SenCliffR])
&& (!sensors[SenChAvailable])
)
{
// Keep turning until the specified angle is reached
if(turning)
{
if(backing_up)
{
if((-distance) > 5)
backing_up = 0;
patrol_room(-200, RadStraight);
}
else
{
if(turn_dir)
{
if(angle > turn_angle)
turning = 0;
patrol_room(200, RadCCW);
}
else
{
if((-angle) > turn_angle)
turning = 0;
patrol_room(200, RadCW);
}
}
}
else if(sensors[SenBumpDrop] & BumpEither) // Check for a bump
{
// Set the turn parameters and reset the angle
if(sensors[SenBumpDrop] & BumpLeft)
turn_dir = 0;
else
turn_dir = 1;
backing_up = 1;
turning = 1;
distance = 0;
angle = 0;
turn_angle = randomAngle();
// Play the bump song
byteTx(CmdPlay);
byteTx(BUMP_SONG);
}
else
{
// Otherwise, patrol_room straight
patrol_room(300, RadStraight);
}
// Flash the leds in sequence
if(++leds_cnt >= 10)
{
leds_cnt = 0;
if(turning)
{
// Flash backward while turning
if(leds_state == 0)
leds_state = 4;
else
leds_state--;
}
else
{
if(leds_state == 4)
leds_state = 0;
else
leds_state++;
}
if(leds_state == 0)
{
// robot Power LED Amber
byteTx(CmdLeds);
byteTx(0x00);
byteTx(128);
byteTx(255);
LEDBothOff;
}
else if(leds_state == 1)
{
// Play LED on
byteTx(CmdLeds);
byteTx(LEDPlay);
byteTx(0);
byteTx(0);
LEDBothOff;
}
else if(leds_state == 2)
{
// Advance LED on
byteTx(CmdLeds);
byteTx(LEDAdvance);
byteTx(0);
byteTx(0);
LEDBothOff;
}
else if(leds_state == 3)
{
// Robot LEDs off, CM left LED on
byteTx(CmdLeds);
byteTx(0x00);
byteTx(0);
byteTx(0);
LED2On;
LED1Off;
}
else if(leds_state == 4)
{
// Robot LEDs off, CM right LED on
byteTx(CmdLeds);
byteTx(0x00);
byteTx(0);
byteTx(0);
LED1On;
LED2Off;
}
}
// wait a little more than one robot tick for sensors to update
delayAndUpdateSensors(20);
}
// Stop driving
patrol_room(0, RadStraight);
// Play end song and wait
delayAndUpdateSensors(500);
byteTx(CmdPlay);
byteTx(END_SONG);
delayAndUpdateSensors(2438);
}
}
}
int main() {
// Set up Create and module
initializeCommandModule();
powerOnRobot();
// Is the Robot on
byteTx(CmdStart);
// Start the create
baud(Baud57600);
// Set the baud rate for the Create and Command Module
defineSongs();
// Define some songs so that we know the robot is on.
byteTx(CmdControl);
// Deprecated form of safe mode. I use it because it will
// turn of all LEDs, so it's essentially a reset.
byteTx(CmdFull);
// We are operating in FULL mode.
// CSCE 274 students: I would make sure the robot stops.
// As a precaution for the robot and your grade.
//Doesn't need to stop for task 1 since it doesn't move.
// Play the reset song and wait while it plays.
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayMs(750);
// Turn the power button on to something. I like red, but here is green.
// CSCE 274 students: The following should (will) be a function that you write.
changePowerLightRed(); //power light now red
// Initialize global variables
uint8_t bumpbyte, bumpLeft, bumpRight;
int rightLEDonInt = 0;
int leftLEDonInt = 1;
//call initial functions
//setup LEDs
setupRightLED();
setupLeftLED();
leftLEDon(); //start with left LED on
// Infinite operation loop
for(;;) {
while (!bumpRight && !bumpLeft) { //no bumps
//no bumps are rightLED on
if (rightLEDonInt == 1 && leftLEDonInt == 0 && !bumpRight && !bumpLeft) {
rightLEDoff();
rightLEDonInt = 0;
leftLEDon();
leftLEDonInt = 1;
} //no bumps and left LED on
else if (rightLEDonInt == 0 && leftLEDonInt == 1 && !bumpRight && !bumpLeft) {
rightLEDon();
rightLEDonInt = 1;
leftLEDoff();
leftLEDonInt = 0;
} //no bumps and LEDs off
else if (rightLEDonInt == 0 && leftLEDonInt == 0 && !bumpRight && !bumpLeft) {
rightLEDon();
rightLEDonInt = 1;
}//no bumps and both LEDs on
else if (rightLEDonInt == 1 && leftLEDonInt == 1 && !bumpRight && !bumpLeft) {
leftLEDon();
leftLEDonInt = 1;
}
delayMs(100); //delay for tenth of a second
//update bump sensors
byteTx(CmdSensors);
byteTx(SenOverC);
bumpbyte = byteRx();
bumpRight = bumpbyte & (1 << 0);
bumpLeft = bumpbyte & (1 << 1);
}
//update bump sensors again
byteTx(CmdSensors);
byteTx(SenOverC);
bumpbyte = byteRx();
bumpRight = bumpbyte & (1 << 0);
bumpLeft = bumpbyte & (1 << 1);
//turn on LEDs according to bump sensors
if (bumpRight) {
rightLEDon();
rightLEDonInt = 1;
} else {
rightLEDoff();
rightLEDonInt = 0;
}
if (bumpLeft) {
leftLEDon();
leftLEDonInt = 1;
} else {
leftLEDoff();
leftLEDonInt = 0;
}
delayMs(10);
if(UserButtonPressed) {
powerOffRobot();
exit(1);
}
}
}
// Declare Global variables
int main(void) {
// Set up Create and module
initializeCommandModule();
powerOnRobot();
// Is the Robot on
byteTx(CmdStart);
// Start the create
baud(Baud57600);
// Set the baud rate for the Create and Command Module
defineSongs();
// Define some songs so that we know the robot is on.
byteTx(CmdControl);
// Deprecated form of safe mode. I use it because it will
// turn of all LEDs, so it's essentially a reset.
byteTx(CmdFull);
// We are operating in FULL mode.
// Play the reset song and wait while it plays.
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayMs(750);
// Turn the power button on to something.
initializeSensors(); // Sets up sensors.
delayMs(200); // Gives the robot time to process.
allowDriving(); // Enables Driving.
enableOrders(); // Enables driveOrders()
initializePlanner(); // Sets up the planner.
decidePlanState(); // Decide on an initial state of the planner.
disableIntegral(); // Integral term only used for state MAKETURN.
//int16_t speed = DRIVESPEED; // Standard speed.
delayMs(500); // Gives the robot time to process all of the above before continuing.
// Infinite operation loop
while(1) {
compileSensors(); // Build the sensor Struct. Automatic Polling.
decidePlanState();
delayMs(50); // The above operations are time-consuming, so wait.
switch (getPlanState()) {
case FINDWALL:
debugLEDOff();
delayMs(10);
break;
case REPOSITION:
buildInfrared();
buoyLights();
doDocking();
break;
case STOP:
driveOrders(0, 0, 50);
debugLEDOff();
delayMs(10);
break;
default:
debugLEDOff(); // Turn on the lights.
delayMs(10);
break;
}
}
}
//play hotline bling
void BLINGBLING(void) {
byteTx(CmdPlay);
byteTx(HOTLINE_BLING);
delayMs(750);
}
// Light only the power LED on the robot.
void debugLEDOff(void) {
byteTx(139);
byteTx(0);
byteTx(0);
byteTx(255);
}
// Light the play and power LEDs on the robot.
void debugLEDFlash(void) {
byteTx(139);
byteTx(0);
byteTx(255);
byteTx(255);
}
/*
This function to keep turning left or right until a
charge state is detected.
The parameter passed to this function decides if it will
turn left or right first.
*/
void findDock(uint8_t isRed) {
//actively looking for dock
lookForDock = 1;
delayMs(1500);
if (docked == 1) {
BLINGBLING();
changePowerLightGreen();
stop();
exit(0);
return;
}
//drive backwords and check for dock
drive(-100);
delayMs(100);
stop();
delayMs(1500);
if (docked == 1) {
BLINGBLING();
changePowerLightGreen();
stop();
exit(0);
return;
}
//begin the turning process
int i,j;
for (i=0; i<3; i++) {
int bound;
if (i == 0) {
bound = 2500;
} else {
bound = 4500;
}
//right if approach from red, left if approach from green
for (j=0; j<bound; j+=500) {
if (isRed) {
driveLR(-25, 25);
} else {
driveLR(25, -25);
}
delayMs(750); //500
stop();
delayMs(1500);
byteTx(CmdSensors);
byteTx(21);
if (docked == 1) {
BLINGBLING();
changePowerLightGreen();
stop();
exit(0);
return;
}
}
//left if approach from red, right if approach from green
for (j=0; j<bound; j+=500) {
if (isRed) {
driveLR(25, -25);
} else {
driveLR(-25, 25);
}
delayMs(750); //500
stop();
delayMs(1500);
byteTx(CmdSensors);
byteTx(21);
if (docked == 1) {
BLINGBLING();
changePowerLightGreen();
stop();
exit(0);
return;
}
}
}
//if still no dock, turn around and reset
//turn(TURN_180_DEGREES);
drive(-100);
delayMs(2000);
stop();
//turn(TURN_180_DEGREES);
//stop();
getBumps();
getDockSenses();
//dock();
//return;
}
