2014/modeautonomous.cpp at master · team3499/2014 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#include "modeautonomous.h"

#include <Timer.h>

ModeAutonomous::ModeAutonomous(DriverStation *ds) : ModeBase(ds){
    OUT("Autonomous Construct");
}
ModeAutonomous::~ModeAutonomous(){
    OUT("Autonomous Destroy");
}

void ModeAutonomous::begin(){
    m_ds->InAutonomous(true);
    OUT("Autonomous Begin");
    compressor->Start();

    mainLights->setMode(ArduinoControl::Autonomous);

    step = 0;
    drivetime = new Timer();

    drivesys->checkDead();
    drivesys->setPID(0.200, 0.001, 0.002);
}
void ModeAutonomous::run(){
    // clear
    airsys->unShootBall();

    switch(step){
    case 0:
        OUT("Autonomous steps start");
        drivesys->setOutputs(350.0, 350.0, 350.0, 350.0);
        drivetime->Start();
        ++step;
        break;
    case 1:
        if(drivetime->HasPeriodPassed(1.0)){
            OUT("Autonomous step 0 done");
            drivesys->tchunk();
            drivetime->Stop();
            ++step;
        } else {
            drivesys->setOutputs(350.0, 350.0, 350.0, 350.0);
        }
        break;
    case 2:
        OUT("Autonomous steps done");
        ++step;
        break;
    }
}
void ModeAutonomous::end(){
    compressor->Stop();
    OUT("Autonomous End");
    m_ds->InAutonomous(false);

    drivesys->tchunk(); // this should already be done... but just in case...
    drivesys->resetPID();
}

const char *ModeAutonomous::typeString(){
    return "Autonomous";
}