bool SimonSays::frameEnded(const Ogre::FrameEvent &evt) {
bool ret = BaseApplication::frameEnded(evt);
//if ( showMode ) { // if its in show mode, activate the spots according to the order in simonOrder
simonTimer += evt.timeSinceLastFrame; // returns time in seconds
if( pause && simonTimer > 0.5 ) {
mSceneMgr->getLight("yellowSpot")->setVisible( false );
mSceneMgr->getLight("redSpot")->setVisible( false );
mSceneMgr->getLight("blueSpot")->setVisible( false );
mSceneMgr->getLight("greenSpot")->setVisible( false );
simonTimer = 0;
pause = false;
}
if( !pause && simonTimer > 1 ) {
simonTimer = 0;
if( simonCounter < simonOrder.size() ) { // as long as there's stuff in the vector there's still spots to light up
turnOnSpot( simonOrder.at(simonCounter) );
playerOrder.push_back( simonOrder.at(simonCounter) );
//simonOrder.erase( simonOrder.begin());
simonCounter++;
pause = true;
} else { // die Spieler-Runde starten
setShowMode(false);
}
} // eof simonTimer > x
//}
if( turnEnded ) {
setShowMode(true); // this turns off all the lights and stuff
}
return ret;
}
void PIDDriver(short target){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
time1[T4] = 0;
float pre_compass = compass();
while(true){
if(time1[T1] < timeout){ // T1 for timeout
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left] = cruise_speed - u;
motor[Right] = cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
time1[T3] = 0; // T3 for beepInterval
}
pre_compass = cur_compass;
time1[T2] = 0; // T2 for sampling frequency
float tmp = signedCompass();
nxtDisplayStringAt(0,31,"%f", tmp);
logCompass(tmp);
}
}
else{
if(!turnOnSpot(target)) return;
}
}
}
void progSkills()
{
//while (SensorValue[bumper] == 0) {}
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);
baseStop();
// Deploy
deploy();
moveLiftAuto(1,3100);
moveLift(20);
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 40) moveStraight(40);
baseStop();
/*
moveStraight(127);
wait1Msec(200);
moveStraight(-127);
wait1Msec(200);
*/
moveStraight(127);
wait1Msec(200);
intake(-1);
moveStraight( -127);
wait1Msec(200);
baseStop();
//intake(-1);
wait1Msec(2000);
moveStraight(50);
wait1Msec(100);
baseStop();
intake(1);
moveLift(-30);
wait1Msec(500);
intake(0);
liftStop();
moveStraight(-40);
wait1Msec(200);
baseStop();
SensorValue[leftBackS] = 0;
SensorValue[rightBackS] = 0;
while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 450)
{
while (SensorValue[rightLiftS] < 4000)
{
moveLift(-70);
turnOnSpot(2,90);
}
liftStop();
}
baseStop();
intake(1);
moveStraight(70);
wait1Msec(1000);
baseStop();
SensorValue[leftBackS] = 0;
SensorValue[rightBackS] = 0;
while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
{
turnOnSpot(2,90);
}
baseStop();
moveStraight(127);
wait1Msec(2000);
baseStop();
while(SensorValue[bumper] == 0) {}
intake(-1);
while(SensorValue[bumper] == 0) {}
intake(0);
// Raise Lift
// Output
while(SensorValue[bumper] == 0) {}
intake (1);
moveStraight(127);
wait10Msec(50);
turnOnSpot(1,60);
wait10Msec(30);
turnOnSpot(2,60);
wait10Msec(30);
baseStop();
SensorValue[leftBackS] = 0;
SensorValue[rightBackS] = 0;
while (((abs(SensorValue[leftBackS])+abs(SensorValue[rightBackS]))/2) < 500)
{
turnOnSpot(1,90);
}
intake(-1);
wait10Msec(100);
}
void redSkyriseAuton()
{
//while (SensorValue[bumper] == 0) {}
deploy();
intake(1);
SensorValue[rightBackS] = 0;
while(abs(SensorValue[rightBackS]) < 200)
{
moveStraight(127);
}
baseStop();
wait1Msec(1000);
SensorValue[rightBackS] = 0;
SensorValue[leftBackS] = 0;
while(((abs(SensorValue[rightBackS]) + abs(SensorValue[leftBackS]))/2) < 500)
{
turnOnSpot(1,90);
}
baseStop();
intake(-1);
wait1Msec(5000);
intake(0);
SensorValue[rightBackS] = 0;
while(abs(SensorValue[rightBackS]) < 100)
{
moveStraight(-90);
}
baseStop();
/*
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 70) moveStraight(-90);
baseStop();
// Deploy
deploy();
//Raise Lift
moveLiftAuto(1,3500);
intake(1);
moveLift(-1);
wait1Msec(300);
moveLift(0);
moveStraight(127);
wait1Msec(1000);
baseStop();
moveLiftAuto(-1,3450);
wait1Msec(500);
moveLiftAuto(1,3000);
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 200)
{
moveStraight(-60);
}
baseStop();
SensorValue[rightBackS] = 0;
SensorValue[leftBackS] = 0;
while(((abs(SensorValue[rightBackS])+abs(SensorValue[leftBackS]))/2) < 480)
{
moveLeftSide(-90);
}
moveLeftSide(1);
wait1Msec(50);
baseStop();
SensorValue[rightBackS] = 0;
while (abs(SensorValue[rightBackS]) < 180)
{
moveStraight(70);
}
baseStop();
moveLiftAuto(-1,3550);
moveStraight(30);
moveLiftAuto(-1,3900);
while(abs(SensorValue[rightBackS]) < 200)
{
moveStraight(-60);
}
baseStop();
//Move Forward
//Move Backward
//Turn around
//Drop Lift
//Score Skyrise*/
}
void PIDDriver(short target){
float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
short u;
time1[T1] = 0;
time1[T2] = 0;
time1[T4] = 0;
float pre_compass = compass();
bool greyPatch = false;
while(true){
if(time1[T1] < timeout){ // T1 for timeout
e = threshold - SensorValue[Light];
e_dot = e - old_e;
E += e;
u = (short)(KP * e + KD * e_dot + KI * E);
old_e = e;
motor[Left] = cruise_speed - u;
motor[Right] = cruise_speed + u;
if(SensorValue[Light] < target){ //target can be make more conservative than threshold
time1[T1] = 0;
}
// When grey patch detected
if(SensorValue[Light] > 38 && SensorValue[Light] < 43){
if(time1[T4] > 300){
greyPatch = true;
stop();
// beep three times
PlayTone(1500, 40);
wait1Msec(800);
PlayTone(1500, 40);
wait1Msec(800);
PlayTone(1500, 40);
wait1Msec(800);
control(rotateSpeed, -rotateSpeed, 800); // 800 is for getting rid of the grey patch
while(SensorValue[Light] > threshold){}
stop();
time1[T1] = 0;
time1[T2] = 0;
time1[T3] = 0;
nMotorEncoder[Left]=0;
nMotorEncoder[Right]=0;
pre_compass = compass();
continue;
}
}
else{
time1[T4] = 0;
}
if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
float cur_compass = compass();
if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
PlayTone(1175, 20);
time1[T3] = 0; // T3 for beepInterval
}
pre_compass = cur_compass;
time1[T2] = 0; // T2 for sampling frequency
if(greyPatch){ // log the compass value from grey patch to the other endpoint
float tmp = signedCompass();
nxtDisplayStringAt(0,31,"%f", tmp);
logCompass(tmp);
}
}
}
else{
if(greyPatch){
if(!turnOnSpot(target, greyPatch)) return; // reach the endpoint other than grey patch, return
}
else{
turnOnSpot(target, greyPatch);
}
}
}
}
