-
Notifications
You must be signed in to change notification settings - Fork 0
/
driveStraight.c
executable file
·68 lines (59 loc) · 1.64 KB
/
driveStraight.c
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
64
65
66
67
68
task pidController() {
pidLastError = 0;
pidIntegral = 0;
while( true ) {
if( pidRunning ) {
if (abs(nMotorEncoder[lW2]) > abs(nMotorEncoder[rW2])) {
pidSensorCurrentValue = nMotorEncoder[rW2];
secondary = -nMotorEncoder[lW2];
difference = abs(secondary) - abs(pidSensorCurrentValue);
driveL(-pidDrive + constant*difference);
driveR(-pidDrive);
}
else {
pidSensorCurrentValue = -nMotorEncoder[lW2];
secondary = nMotorEncoder[rW2];
difference = abs(secondary) - abs(pidSensorCurrentValue);
driveL(-pidDrive);
driveR(-pidDrive + constant*difference);
}
pidError = pidSensorCurrentValue - pidRequestedValue;
if( pid_Ki != 0 ) {
if( abs(pidError) < PID_INTEGRAL_LIMIT )
pidIntegral = pidIntegral + pidError;
else
pidIntegral = 0;
}
else
pidIntegral = 0;
pidDerivative = pidError - pidLastError;
pidLastError = pidError;
pidDrive = (pid_Kp * pidError) + (pid_Ki * pidIntegral) + (pid_Kd * pidDerivative);
if ( pidDrive > PID_DRIVE_MAX )
pidDrive = PID_DRIVE_MAX;
if ( pidDrive < PID_DRIVE_MIN )
pidDrive = PID_DRIVE_MIN;
}
else {
pidError = 0;
pidLastError = 0;
pidIntegral = 0;
pidDerivative = 0;
}
// Run at 50Hz
wait1Msec( 25 );
}
}
void go(float inches, float s = 85) {
resetMotorEncoder(lW2);
resetMotorEncoder(rW2);
pidRequestedValue = inches * ticksPerInch;
}
task driveTarget() {
while (true) {
if (abs(pidError) < dt)
atTarget = true;
else
atTarget = false;
}
}