Non-Blocking-Driving-Program/Chassis.cpp at main · jlopez-codes/Non-Blocking-Driving-Program · 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
#include "Chassis.h"

void Chassis::driveDistance(float inches)
{
    int countFinal = inches*CPR/(PI*wheelDiameter); //calculate counts needed to cover distance
    encoders.getCountsAndResetLeft(); //Use left wheel to determine distance, set Encoder to zero
    motors.setEfforts(100,100);
    while(encoders.getCountsLeft()< countFinal) //check counts on left wheel
    {
        //wait
    }
    motors.setEfforts(0,0);
}

bool Chassis::doneDriving()
{
    targetCount = CPR = wheelDiameter;


    }

}

void Chassis::turnAngle(float degrees)
{
    int countFinal = degrees*CPR*wheelTrack/(360.0*wheelDiameter); // calculate counts needed to turn angle
    encoders.getCountsAndResetLeft(); //Use left wheel to determine angle, set Encoder to Zero
    motors.setEfforts(100, -100);
    while (encoders.getCountsLeft() < countFinal) //Check counts on left wheel
    {
        //wait
    }
    motors.setEfforts(0,0);
}

Chassis::Chassis()
{

};

void Chassis::pauseDriving()
//if the Paused button is is pressed, set motors to 0.
{
    if( chassis.paused == true)
    {
        motors.setEfforts(100,100);
        motors.setEfforts(0,0);
    }
}

void Chassis::resumeDriving()
//if the Resume button is pressed, set motors back to (100,100)
{
    if(chassis.paused == false)
    {
        motors.setEfforts(0,0);
        motors.setEfforts(100,100;)
    }
}