Created
January 12, 2017 15:12
-
-
Save anonymous/b2732780abf97966aca9147daddd4744 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#pragma config(Sensor, dgtl1, f1c, sensorTouch) | |
#pragma config(Sensor, dgtl2, f2c, sensorTouch) | |
#pragma config(Sensor, dgtl3, f3c, sensorTouch) | |
#pragma config(Sensor, dgtl4, s, sensorSONAR_cm) | |
#pragma config(Sensor, dgtl7, f1i, sensorLEDtoVCC) | |
#pragma config(Sensor, dgtl8, f2i, sensorLEDtoVCC) | |
#pragma config(Sensor, dgtl9, f3i, sensorLEDtoVCC) | |
#pragma config(Motor, port2, d1, tmotorVex393_HBridge, openLoop) | |
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// | |
// <configuration> | |
float K_P = 5.6; // Proportional coefficient | |
float K_I = 0.001; // Integral coefficient | |
float K_D = 0.3; // Derivative coefficient | |
int K_DT = 250; // Time difference between PID iterations | |
float ERROR_THRESH = 1.1; // Target threshold | |
int FLOOR_HEIGHT = 9; // Height of a floor, in CM | |
int FLOOR_1_OFFSET = 3; // Distance of the first floor from the ultrasonic sensor | |
int IDLE_TIME = 10000; // Idle time threshold | |
// </configuration> | |
int integral = 0; // PID integral value | |
int lastError = 0; // Error value in the previous iteration | |
int clamp(int n, int lower, int upper); // Clamps a value between two bounds | |
void out(int power); // Sets the motor output | |
void pid(float error); // PID controller implementation | |
float error(int expected, int actual); // Heuristic error measuring function | |
void ledUpdate(int floorNum); // Update floor indicator LEDs | |
void goToFloor(int floorNum); // Goes to a floor and blocks until the floor is reached | |
task main() { | |
int idleSince = nSysTime; // Store current system time as time of last action | |
while (true) { | |
if (SensorValue[f1c] == 1) { // Check if floor buttons are pressed | |
goToFloor(0); // And go to the respective floor | |
idleSince = nSysTime; // Then reset the idle time | |
} else if (SensorValue[f2c] == 1) { // Ditto | |
goToFloor(1); | |
idleSince = nSysTime; | |
} if (SensorValue[f3c] == 1) { // Ditto | |
goToFloor(2); | |
idleSince = nSysTime; | |
} else { // If no floor button is pressed | |
if (nSysTime - idleSince > IDLE_TIME) // Check if idle time exceeds threshold | |
goToFloor(0); // Then go back to the ground floor | |
} | |
} | |
} | |
int clamp(int n, int lower, int upper) { | |
return n < lower ? lower : n > upper ? upper : n; // If n is less than lower, return lower. Otherwise, if n is less than upper, return upper. Otherwise, return n | |
} | |
void out(int power) { | |
if (power < 0) | |
power = clamp(power, -96, -18); | |
else if (power > 0) | |
power = clamp(power, 18, 96); | |
motor[d1] = power; // Set motor d1's output to negative power | |
} | |
void pid(float error) { | |
integral += error * K_DT; // Add the current error value to the integral cumulator | |
float pTerm = K_P * error; // Calculate proportional term | |
float iTerm = K_I * integral; // Calculate integral term | |
float dTerm = K_D * (error - lastError) / K_DT; // Calculate derivative term | |
out(pTerm + iTerm + dTerm); // Set output to clamped value of u(t) | |
lastError = error; // Store error value to lastError for calculating derivative later | |
} | |
float error(int expected, int actual) { | |
return actual - expected; // Difference between setpoint and measured value | |
} | |
void ledUpdate(int floorNum) { | |
SensorValue[f1i] = SensorValue[f2i] = SensorValue[f3i] = 0; // Turn off all LEDs | |
switch (floorNum) { // Switch on the new floor number | |
case 0: // If the floor is appropriate | |
SensorValue[f1i] = 1; // Turn on the corresponding LED | |
break; | |
case 1: // Ditto | |
SensorValue[f2i] = 1; | |
break; | |
case 2: // Ditto | |
SensorValue[f3i] = 1; | |
break; | |
} | |
} | |
void goToFloor(int floorNum) { | |
integral = 0; // Reset integral and last error value variables | |
lastError = 0; | |
int setPoint = floorNum * FLOOR_HEIGHT + FLOOR_1_OFFSET; // Determine PID setpoint | |
int errorValue; // Declare error value variable | |
while (true) { | |
errorValue = error(setPoint, SensorValue[s]); // Calculate error value | |
if (abs(errorValue) <= ERROR_THRESH) // If within the error threshold... | |
break; // ...break the PID loop | |
pid(errorValue); // Execute PID iteration | |
waitInMilliseconds(K_DT); // Wait for next PID iteration | |
} | |
out(0); // Stop the motors | |
ledUpdate(floorNum); // Update the LEDs | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment