int16_t main(void)
{
initRobotBase();
powerON();
setLEDs(0b111111);
mSleep(1000);
setLEDs(0b000000);
mSleep(500);
uint8_t runningLight = 1;
for (uint8_t i = 0; i < 6; i++){
for (uint8_t j = 0; j< 3;j++)
destination[j] = destinationarray[i][j];
//mainloopOpen();
mainloopClosed();
fancyled(&runningLight);
/*for (uint8_t j = 0; j< 3;j++)
source[j] = destination[j];*/
mSleep(2000);
}
task_RP6System();
while(true)
{
fancyled(&runningLight);
if (motioncomplete == 0)
moveAtSpeed(0,0);
mSleep(200);
task_RP6System();
}
return 0;
}
void mainloopOpen()
{
motioncomplete=1;
while(motioncomplete == 1) {
double delta_x=0.0, delta_y=0.0,pho=0.0;
//Odometry(); //should comment for open loop controller
delta_x = destination[0] - source[0];
delta_y = destination[1] - source[1];
pho = sqrt(pow(delta_x,2)+pow(delta_y,2));
double alpha = atan2(delta_y,delta_x);
if (abs(alpha*100) > 0){
alpha = (alpha*180)/M_PI;
//alpha = (alpha < 0 ? -(alpha +180):alpha);
}
/*writeIntegerLength((int16_t)delta_y, DEC,16);
writeString("\n");
writeIntegerLength((int16_t)delta_x, DEC,16);
writeString("\n");
writeIntegerLength((int16_t)alpha, DEC,16);
writeString("\n");*/
task_RP6System();
/*REPLACE THIS CONTROLLER*/
OpenLoopController((int16_t)alpha,pho,0);
//SiegwartController(alpha,pho,0);
task_RP6System();
//motioncomplete=0; //debug ... do not forget to remove :-)
mSleep(200);
}
}
void mainloopClosed()
{
motioncomplete = 1;
while(motioncomplete == 1){
double delta_x=0.0, delta_y=0.0,pho=0.0;
Odometry();
delta_x = destination[0] - source[0];
delta_y = destination[1] - source[1];
pho = sqrt(pow(delta_x,2)+pow(delta_y,2));
double alpha = atan2(delta_y,delta_x);
/*if (abs(alpha*100) > 0){
alpha = (alpha*180)/M_PI;
}*/
task_RP6System();
SiegwartController(alpha,pho,0);
task_RP6System();
task_motionControl();
mSleep(200);
}
}
int main(void) {
initRobotBase();
setLEDs(0b111111);
mSleep(1500);
setLEDs(0b000000);
// Set Bumpers state changed event handler:
BUMPERS_setStateChangedHandler(bumpersStateChanged);
powerON(); // Turn Encoders, Motor Current Sensors
// ATTENTION: Automatic Motor control will not work without this!
/* RP6 SAGAN GENERATED COMMANDS START */
/*{SAGAN1_COMMANDS_HERE}*/
/* RP6 SAGAN GENERATED COMMANDS STOP */
stop();
moveAtSpeed(0, 0);
BUMPERS_setStateChangedHandler(BUMPERS_stateChanged_empty);
setLEDs(0b000000);
while (true) {
statusLEDs.LED2 = !statusLEDs.LED2; // Toggle LED bit in LED shadow register...
statusLEDs.LED5 = !statusLEDs.LED5;
updateStatusLEDs();
mSleep(500);
task_RP6System();
}
return 0;
}
int main(void)
{
initRobotBase();
setLEDs(0b111111);
mSleep(2500);
setLEDs(0b100100);
BUMPERS_setStateChangedHandler(bumpersStateChanged);
powerON();
while(true)
{
behaviourController();
task_RP6System();
}
return 0;
}
/**
* Here we react on any obstacle that we may hit.
* If any of the bumpers detects an obstacle, we stop the motors and start
* LED blink.
*/
void bumpersStateChanged(void) {
if (bumper_left || bumper_right) {
stop();
moveAtSpeed(0, 0); // stop moving!
setLEDs(0b111111);
updateStatusLEDs();
mSleep(500);
while (true) {
statusLEDs.LED2 = !statusLEDs.LED2; // Toggle LED bit in LED shadow register...
statusLEDs.LED5 = !statusLEDs.LED5;
updateStatusLEDs();
mSleep(500);
task_RP6System();
}
}
}
