void setup()
{
//to prevent fuse from blowing
motor.speed(leftWheelPin, 0) ;
motor.speed(rightWheelPin, 0) ;
//attach driving servo
RCServo1.attach(RCServo1Output) ;
RCServo1.write(neutral) ;
//set ports to inputs
portMode(0, INPUT) ;
portMode(1, INPUT) ;
Serial.begin(9600);
while(1){
delay(50);
while( !(stopbutton()) ){
delay(50);
LCD.setCursor(0,0); LCD.clear(); LCD.home() ;
LCD.print("kP");
LCD.print(" ");
LCD.print(kP);
kP = knob(knob_one)/10;
LCD.print(" ");
LCD.setCursor(0,1);
LCD.print("kD");
LCD.print(" ");
LCD.print(kD);
kD = knob(knob_two)/10;
if (startbutton())
{
goto loopstart;
}
}
delay(50);
while( !(stopbutton())) {
delay(50);
LCD.setCursor(0,0); LCD.clear(); LCD.home() ;
LCD.print("motorSpeed");
LCD.print(" ");
LCD.print(motorSpeed);
motorSpeed = knob(knob_two);
LCD.setCursor(0,1);
LCD.print("Adjust");
LCD.print(" ");
LCD.print(knob(knob_one));
motorAdjustment = knob(knob_one)/512;
if (startbutton())
{
goto loopstart;
}
}
}
loopstart: ;
}
// Determines if the stop button is being pressed
// Optional: Debounces the button for the specified number of milliseconds
bool StopButton(int debounceTime = 80)
{
if(!stopbutton()) return false;
delay(debounceTime);
return stopbutton();
}
int main()
{
sf::RenderWindow window({ 640, 480 }, "Motion Playback Example"); // create the SFML window
window.setVerticalSyncEnabled(true);
// Create some buttons to control the playback
sf::VertexArray playbutton(sf::PrimitiveType::Triangles);
playbutton.append({ { 255, 350 }, sf::Color::Green });
playbutton.append({ { 275, 360 }, sf::Color::Green });
playbutton.append({ { 255, 370 }, sf::Color::Green });
sf::VertexArray pausebutton(sf::PrimitiveType::Quads);
pausebutton.append({ { 305, 350 }, sf::Color::Blue });
pausebutton.append({ { 310, 350 }, sf::Color::Blue });
pausebutton.append({ { 310, 370 }, sf::Color::Blue });
pausebutton.append({ { 305, 370 }, sf::Color::Blue });
pausebutton.append({ { 320, 350 }, sf::Color::Blue });
pausebutton.append({ { 325, 350 }, sf::Color::Blue });
pausebutton.append({ { 325, 370 }, sf::Color::Blue });
pausebutton.append({ { 320, 370 }, sf::Color::Blue });
sf::VertexArray stopbutton(sf::PrimitiveType::Quads);
stopbutton.append({ { 355, 350 }, sf::Color::Red });
stopbutton.append({ { 375, 350 }, sf::Color::Red });
stopbutton.append({ { 375, 370 }, sf::Color::Red });
stopbutton.append({ { 355, 370 }, sf::Color::Red });
mt::DataSource datasource; // create the data source from which playback will happen
if (!datasource.LoadFromFile("{video file path}")) // load a file into the data source
return EXIT_FAILURE;
mt::SFMLAudioPlayback audioplayback(datasource); // create an audio playback from our data source
mt::SFMLVideoPlayback videoplayback(datasource); // create a video playback from our data source
// scale video to fit the window
videoplayback.setScale(640.f / static_cast<float>(datasource.GetVideoSize().x), 480.f / static_cast<float>(datasource.GetVideoSize().y));
// standard SFML game and event loops
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed)
window.close();
else if (event.type == sf::Event::MouseButtonPressed)
{
if (playbutton.getBounds().contains(window.mapPixelToCoords({ event.mouseButton.x, event.mouseButton.y })))
datasource.Play(); // play button was pressed - so start the playback
else if (pausebutton.getBounds().contains(window.mapPixelToCoords({ event.mouseButton.x, event.mouseButton.y })))
datasource.Pause(); // pause button was pressed - so pause the playback
else if (stopbutton.getBounds().contains(window.mapPixelToCoords({ event.mouseButton.x, event.mouseButton.y })))
datasource.Stop(); // stop button was pressed - stop the playback
}
}
datasource.Update(); // update the data source - this is required for any playbacks
window.clear();
window.draw(videoplayback); // draw the video playback
// draw the buttons
window.draw(playbutton);
window.draw(pausebutton);
window.draw(stopbutton);
window.display();
}
}
void loop()
{
RCServo1.write(angle_neutral);
collector_switchState = LOW;
if (timeTicker == 50) {
timeTicker = 0;
if (startbutton() && !startButtonState) {
if (menuState == 3) {
menuState = 1;
} else {
menuState++;
}
editable = false;
startButtonState = true;
delay(500);
} else if (stopbutton() && !stopButtonState) {
editable = true;
stopButtonState = true;
} else {
stopButtonState = false;
startButtonState = false;
}
LCD.clear();
// Menu System
// 1:
switch (menuState) {
case 1:
LCD.print("Rest: ");
userSet = (int)(knob(6) * 180.0/1024.0);
if (userSet != angle_neutral && editable){
angle_neutral = userSet;
}
LCD.print(angle_neutral, DEC);
LCD.setCursor(0,1);
LCD.print("Collect?: Y/N ");
userSet = (int)(knob(7) * 2.0/1024.0);
if(userSet != collect_status && editable){
collect_status = userSet;
}
LCD.print(collect_status, DEC);
break;
case 2:
LCD.print("Min: ");
userSet = (int)(knob(6) * 180.0/1024.0);
if(userSet != angle_min && editable){
angle_min = userSet;
}
LCD.print(angle_min, DEC);
LCD.setCursor(0,1);
LCD.print("Max: ");
userSet = (int)(knob(7) * 180.0/1024.0);
if(userSet != angle_max && editable){
angle_max = userSet;
}
LCD.print(angle_max, DEC);
break;
case 3:
LCD.print("TestRange: ");
userSet = 1.5*(int)(knob(6) * 180.0/1024.0);
if(userSet != angle_neutral && editable){
if (userSet >= 180){
angle_neutral = 180;
} else{
angle_neutral = userSet;
}
}
LCD.print(angle_neutral, DEC);
break;
}
}
if(collect_status == 1){
collector_switchState = digitalRead(COLLECTOR_SWITCHPIN);
if(collector_switchState){
RCServo1.write(angle_min);
delay(200);
RCServo0.write(angle_max);
delay(200);
pusher_activate();
RCServo1.write(angle_neutral);
collector_switchState = false;
}
}
timeTicker++;
}
void Shoot(uint16 diff)
{
switch (shooterState)
{
case ssAim: // controls if robot is to shoot or to move back
StartShooterControl();
if (beginBackIR == -1)
beginBackIR = GetRearAveragedSum();
// try to load more balls
if (!IsLoaded())
{
StopMoving();
GuardUp();
if (UpdateTimer(diff, loadTimer, cLoadTimer)) // ran out of time to load new ball - have no balls
shooterState = ssAlignBack;
sDebugPhase = "Loading";
}
else
{
loadTimer = cLoadTimer;
if (GetTargAveragedSum() < cTargOPMinThresh) //need to coarse adj
{
if (doMidAdjust)
{
StopMoving();
doMidAdjust = false;
midAjustComplete = true;
/*
if (PID::OPDriveAdjustCompleted(sCalibratedBackIRVal * cIRFalloffCenter))
{
StopMoving();
doMidAdjust = false;
midAjustComplete = true;
}
PID::OPDriveUpdate(diff, sCalibratedBackIRVal * cIRFalloffCenter);
*/
}
else
{
if (aimState == asCW)
{
//RotateCW(cAimSpeed);
SetMotorSpeed(tLeftDriveMotor, 100 + cAimSpeed);
SetMotorSpeed(tRightDriveMotor, 100 - cAimSpeed);
}
else if (aimState == asCCW)
{
//RotateCCW(cAimSpeed);
SetMotorSpeed(tLeftDriveMotor, 100 - cAimSpeed);
SetMotorSpeed(tRightDriveMotor, 100 + cAimSpeed);
}
}
if (coarseMaxThreshPassed) //coarse adj control
{
if (GetRearAveragedSum() < beginBackIR * (aimState == asCW ? cTurnIRFalloffCW : cTurnIRFalloffCCW))
{
//switch
aimState = (aimState == asCW ? asCCW : asCW);
coarseMaxThreshPassed = false;
midAjustComplete = false;
doMidAdjust = false;
}
sDebugPhase = "Coarse 2";
}
else
{
if (GetRearAveragedSum() >= beginBackIR * 0.75f)
{
if (!midAjustComplete)
doMidAdjust = true;
coarseMaxThreshPassed = true;
}
sDebugPhase = "Coarse 1";
}
}
else // fine
{
midAjustComplete = false;
doMidAdjust = false;
if (GetRearAveragedSum() >= beginBackIR * 0.75f)
coarseMaxThreshPassed = true;
if (PID::TargOPAdjustCompleted())
{
StopMoving();
shooterState = ssFire;
//sAimTmr = cMaxAimDurationTimer;
}
PID::OPUpdate(true, diff, PID::OPPGainFront, 0, 0);
if (IsLoaded())
if (UpdateTimer(diff, sAimTmr, cMaxAimDurationTimer))
{
StopMoving();
shooterState = ssFire;
}
sDebugPhase = "Fine Adj";
}
}
break;
case ssFire:
StopMoving();
if (shooterReady)
GuardDown();
if (!IsLoaded())
if (UpdateTimer(diff, sShootTmr, cShootTimer))
{
sAimTmr = cMaxAimDurationTimer;
shooterState = ssAim;
}
sDebugPhase = "Fire";
break;
case ssAlignBack:
StopShooterControl();
if (PID::RearOPAdjustCompleted())
{
StopMoving();
shooterState = ssAim;
robotState = rsNavigateBack;
beginBackIR = -1;
sDebugPhase = "******************";
}
PID::OPUpdate(false, diff, PID::OPPGainBack, 0, 0);
sDebugPhase = "Align Back";
break;
}
if (UpdateTimer(diff, LCDTimer, LCD_UPDATE_TIMER))
{
LCD_PRINTNEW("S:");
LCD.print(sDebugPhase);
LCD.setCursor(0, 1);
switch (shooterState)
{
case ssAim:
LCD.print("A ");
LCD.print(GetAveragedLeftTargOPReading());
LCD.print(" ");
LCD.print(GetAveragedRightTargOPReading());
LCD.print(" ");
LCD.print(beginBackIR * cTurnIRFalloffCW);
LCD.print(" ");
LCD.print(GetRearAveragedSum());
break;
case ssFire:
LCD.print("SPD ");
LCD.print(shooterTopFreq);
LCD.print(" ");
LCD.print(shooterBotFreq);
LCD.print(" ");
break;
case ssAlignBack:
LCD.print("B ALGN ");
LCD.print(GetLeftRearOPReading());
LCD.print(" ");
LCD.print(GetRightRearOPReading());
LCD.print(" ");
break;
}
}
if (stopbutton())
{
StopShooterControl();
shooterTopFreq = SetConstant("Shooter Top Freq", cShooterTopFreq, 1/2.0f);
shooterBotFreq = SetConstant("Shooter Bot Freq", cShooterBotFreq, 1/2.0f);
SetConstant("Right OP Gain", cFrontRightOPGain, 1/20.0f);
StartShooterControl();
}
}
