-
-
Save phantamanta44/78e3c2dccd7d628e53ef868b7df9a8a2 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 | |
/** | |
* Clamps a value inclusively between two bounds. | |
*/ | |
int clamp(int n, int lower, int upper); | |
/** | |
* Sets the motor output. | |
*/ | |
void out(int power); | |
/** | |
* Executes one iteration of the PID algorithm. | |
*/ | |
void pid(float error); | |
/** | |
* Calculates an error value for a setpoint and measured value. | |
*/ | |
float error(int expected, int actual); | |
/** | |
* Updates floor indicator LEDs. | |
*/ | |
void ledUpdate(int floorNum); | |
/** | |
* Instructs the elevator to travel to a level and blocks until the operation is finished. | |
*/ | |
void goToFloor(int floorNum); | |
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; | |
} | |
void out(int power) { | |
if (power < 0) // Threshold the power level so the motor still drives at lower power levels | |
power = clamp(power, -96, -18); | |
else if (power > 0) | |
power = clamp(power, 18, 96); | |
motor[d1] = power; // Set motor d1's output to 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