//Main
void main(void) {
initBoard();
backlightOn();
//Configure interrupts
//Interrupt on RB1 = SW_E
OpenRB1INT(PORTB_CHANGE_INT_ON & FALLING_EDGE_INT & PORTB_PULLUPS_OFF & PORTB_INT_PRIO_HIGH);
//Interrupt on RB0 = SW_W
OpenRB0INT(PORTB_CHANGE_INT_ON & FALLING_EDGE_INT & PORTB_PULLUPS_OFF & PORTB_INT_PRIO_LOW);
//Enable button input => not needed, see dwengoBoard.c->initBoard();
TRISB = 0xFF;
//Init motors
initializeMotors();
initializeSensors();
while (TRUE)
{
if(SW_N == 0)
{
mode = 0;
clearLCD();
printStringToLCD("Doing moves brah", 1, 0);
leftMotor(700);
rightMotor(700);
delay_s(4);
leftMotor(1000);
rightMotor(700);
delay_s(4);
leftMotor(700);
rightMotor(1000);
delay_s(4);
leftMotor(-700);
rightMotor(700);
delay_s(4);
}
else if(mode == 1)
{
//Light eating
}
else if(SW_S == 0)
{
//Start light eating
//mode = 1;
clearLCD();
printStringToLCD("Going to send string", 0, 0);
initializeRS232();
sendData();
}
else
{
//printStringToLCD("Cool story bro", 0, 0);
}
}
}
/**
* pivot clockwise at a given speed for a given time (millis)
* float speed from -100 to 100
* int millis number of milliseconds
*/
void pivot(int speed, int millis)
{
leftMotor(speed);
rightMotor(-speed);
wait1Msec(millis);
leftMotor(0);
rightMotor(0);
}
void YourLowPriorityISRCode()
{
//Interrupt to stop motors
printStringToLCD("LOWPRI", 1, 0);
leftMotor(0);
rightMotor(0);
}
void YourHighPriorityISRCode()
{
//Interrupt to send Q table
printStringToLCD("HIGHPRI", 1, 0);
leftMotor(0);
rightMotor(0);
}
/**
* Drive at a speed for a specific distance (specified in metres)
*/
void driveDistance(float speed, float distance)
{
float kLag = 0.9; // scale back a motor that is running too fast
float currentDistance = 0.0;
// zero encoder values
nMotorEncoder[motorLeft] = 0;
nMotorEncoder[motorRight] = 0;
wait1Msec(5);
// drive while the current distance is less than the target distance
while(currentDistance <= distance)
{
if(abs(nMotorEncoder[motorLeft]) < abs(nMotorEncoder[motorRight]))
{
// if left motor is too slow
leftMotor(speed);
rightMotor(speed * kLag);
}
else if(abs(nMotorEncoder[motorRight]) < abs(nMotorEncoder[motorLeft]))
{
// if right motor is too slow
leftMotor(speed * kLag);
rightMotor(speed);
}
else
{
leftMotor(speed);
rightMotor(speed);
}
wait1Msec(5); // wait 5 millis to allow encoders to update
currentDistance = (abs(nMotorEncoder[motorLeft]) + abs(nMotorEncoder[motorRight]))/2;
}
// stop motors
leftMotor(0);
rightMotor(0);
}
/**
* Drive at a speed for a time (specified in milliseconds)
* int speed from -100 to 100
* int millis number of milliseconds to drive for
*/
void driveTime(int speed, int millis)
{
float kLag = 0.9;
// zero encoder values to ensure driving in a straight line
nMotorEncoder[motorLeft] = 0;
nMotorEncoder[motorRight] = 0;
clearTimer(T1);
while(time1[T1] < millis)
{
if(abs(nMotorEncoder[motorLeft]) < abs(nMotorEncoder[motorRight]))
{
// if left motor is too slow
leftMotor(speed);
rightMotor(speed * kLag);
}
else if(abs(nMotorEncoder[motorRight]) < abs(nMotorEncoder[motorLeft]))
{
// if right motor is too slow
leftMotor(speed * kLag);
rightMotor(speed);
}
else
{
leftMotor(speed);
rightMotor(speed);
}
wait1Msec(5);
}
// stop motors
leftMotor(0);
rightMotor(0);
}
void Shieldbot::drive(char left, char right){
rightMotor(right);
leftMotor(left);
}
void Shieldbot::backward(){
leftMotor(-128);
rightMotor(-128);
}
void Shieldbot::forward(){
leftMotor(127);
rightMotor(127);
}
void Mobot::forward(int speed) {
enableMotors();
leftMotor(0, speed);
rightMotor(0, speed);
}
void Mobot::turnLeft() {
rightMotor(0, 255);
digitalWrite(_E2, LOW);
}
void Mobot::spinRight(int speed) {
enableMotors();
leftMotor(0, speed);
rightMotor(1, speed);
}
/* Function: rotateMotor
------------------------
Rotate Bot clockwise or counter-clockwise
*/
void rotateMotor(signed int RPMdir1)
{
leftMotor(RPMdir1);
rightMotor(RPMdir1);
}
/* Function: translateMotor
-------------------------
Move Bot straight forward/backwards
*/
void translateMotor(signed int RPMdir)
{
leftMotor(-RPMdir);
rightMotor(RPMdir);
}
void biWheel6::drive(boolean mtr, int spd){
_mtr = mtr;
_spd = spd;
if ( _mtr == 0 ){ rightMotor(_spd);}
else if ( _mtr == 1 ){ leftMotor(_spd);}
}
