task main()
{
StartTask(getHeading);
wait1Msec(500);
while(true) {
MoveRobot(currHeading, 60, 70);
wait1Msec(10);
}
}
/*
Main task
*/
task main () {
StartTask(getHeading);
wait1Msec(500);
while (true) {
// This is where the magic happens. It's so much simpler than you would think.
MoveRobot(currHeading, 60, 70);
wait1Msec(10);
}
}
task main ()
{
// This is the struct that holds all the info on all buttons and joypads/sticks
tPSP controller;
int angle;
int speed;
int rotation;
while (true)
{
// Read the state of the buttons
PSPV4readButtons(PSPNXV4, controller);
// Two controls are used:
// The left joystick controls speed and direction
// The right joystick controls rotational speed
// Calculate the angle we need to travel at using simple geometry
angle = radiansToDegrees(atan2(-controller.joystickLeft_x, -controller.joystickLeft_y));
// This is the length of the vector, which is based on the amount we've moved the
// joystick in the X and Y directions
speed = sqrt((controller.joystickLeft_x * controller.joystickLeft_x) +
(controller.joystickLeft_y*controller.joystickLeft_y));
// Rotation speed is controlled by the right joystick
rotation = -controller.joystickRight_x;
nxtDisplayCenteredTextLine(1, "%d:%d", controller.joystickLeft_x, controller.joystickLeft_y);
nxtDisplayCenteredTextLine(3, "%d:%d", controller.joystickRight_x, controller.joystickRight_y);
nxtDisplayCenteredTextLine(5, "%d", angle);
nxtDisplayCenteredTextLine(6, "%d", speed);
nxtDisplayCenteredTextLine(7, "%d", rotation);
MoveRobot(angle, speed, rotation);
wait1Msec(100);
}
PlaySound(soundBeepBeep);
MoveRobot(0, 0, 0);
while (bSoundActive) EndTimeSlice();
wait1Msec(100);
}
void StopRun(void) {
if (bRecordFlag)
MoveRobot(XSPEED, 50, 0, 150, 0, ACCELERATION);
else if (bExplorationFlag) {
MoveRobot(XSPEED, 500, 0, EXPLORATION_SPEED, 0, ACCELERATION);
bExplorationFlag = FALSE;
} else if (bFastFlag) {
MoveRobot(XSPEED, 550, 0, 0, 0, ACCELERATION);
bFastFlag = FALSE;
} else {
MoveRobot(XSPEED, 100, 0, EXPLORATION_SPEED, 0, ACCELERATION);
// MoveRobot(XSPEED, 2500, 0, EXPLORATION_SPEED, 0, ACCELERATION);
}
StopRobot();
DisableMotor();
bRunFlag = FALSE;
bStopFlag = FALSE;
}
void __attribute__((interrupt, auto_psv)) _CNInterrupt(void) {
//change interrupt for SW1
while(PORTBbits.RB5==0);//while SW1 press, dont do anything
if(PORTBbits.RB5==1)//if SW1 is no longer pressed
{ state++;}//switch state
if(state==4)//if it was previously in state 3(Backwards), go back to state 0 ( IDLE)
{ state=0;}
MoveRobot(state);//call the function that gives direction to the robot
IFS1bits.CNIF = 0;//set flag down
}
void FeedForwardTest(void) {
EnableMotor();
while (1) {
printf("1a");
MoveRobot(XSPEED, 500, 0, 500, 0, 2000);
printf("2a");
MoveRobot(WSPEED, -90, 0, 500, 0, 2000);
printf("3a");
MoveRobot(XSPEED, 500, 0, 500, 0, 2000);
printf("4a");
MoveRobot(WSPEED, -90, 0, 500, 0, 2000);
printf("5a");
MoveRobot(XSPEED, 500, 0, 500, 0, 2000);
printf("6a");
MoveRobot(WSPEED, -90, 0, 500, 0, 2000);
printf("7a");
MoveRobot(XSPEED, 500, 0, 500, 0, 2000);
printf("8a");
MoveRobot(WSPEED, -90, 0, 500, 0, 2000);
printf("9a");
}
}
static void mainTask(void* pvParameters)
{
(void)pvParameters;
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(500);
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(500);
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(500);
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(500);
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(500);
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(500);
controller_Init();
EnWheelMotor();
float maxLinearSpeed = 800;
float endLinearSpeed = 0;
float maxAngularSpeed = 500;
float accX = 2200;
float accW = 8000;
MoveRobot(XSPEED, 5000, 0, maxLinearSpeed, 0, accX);
#if 0
for(int i = 0; i < 8; i++)
{
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW);
}
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, endLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
#endif
#if 0
for(int i = 0; i < 4; i++)
{
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW);
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, 90, 0, maxAngularSpeed, 0, accW);
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW);
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW);
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW);
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, maxLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
MoveRobot(WSPEED, -90, 0, maxAngularSpeed, 0, accW);
}
SetMoveCommand(XSPEED, 180, 0, maxLinearSpeed, endLinearSpeed, accX);
WaitDist(XSPEED, 150);
buzzerbsp_beep(ON);
vTaskDelay(10);
buzzerbsp_beep(OFF);
while(!EndOfMove(XSPEED));
#endif
for(;;)
{
ledsbsp_toogleOutputLed(LED1);
vTaskDelay(300);
}
}
