00001 #include "steering_throttle.h"
00002
00003
00004
00005 static int proportional_term = 0, integeral_term = 0, derivative_term = 0;
00006 static int delta_previous = 0, delta_current = 0;
00007
00008 static const float K_P = 0.125, K_I = 0.08, K_D = 0.4;
00009
00010 static short steering_pulsout_pin, throttle_pulsout_pin;
00011
00012
00013 int initializeSteeringAndThrottleOut(int steering_pin, int throttle_pin) {
00014
00015 steering_pulsout_pin = steering_pin;
00016 throttle_pulsout_pin = throttle_pin;
00017 OUTPUT(steering_pulsout_pin);
00018 OUTPUT(throttle_pulsout_pin);
00019 LOW(steering_pulsout_pin);
00020 LOW(throttle_pulsout_pin);
00021 WAIT(1);
00022 if(IN(steering_pulsout_pin) || IN(throttle_pulsout_pin)) {
00023 INPUT(steering_pulsout_pin);
00024 INPUT(throttle_pulsout_pin);
00025 return -1;
00026 } else return 0;
00027 }
00028
00029
00030 int pulsoutSteering(int steering_setpoint) {
00031 static int steering_pulse_duration = 0;
00032
00033 if(steering_pulse_duration - STEERING_ZERO != steering_setpoint)
00034 steering_pulse_duration += ceilf(((float)steering_setpoint
00035 - (float)(steering_pulse_duration - STEERING_ZERO)) / 2.0);
00036
00037 PULSOUT (steering_pulsout_pin, steering_pulse_duration);
00038 return steering_pulse_duration;
00039 }
00040
00041 int pulsoutThrottle(int throttle_setpoint, int normalized_counts) {
00042 static int throttle_pulse_duration = 0;
00043
00044 proportional_term = throttle_setpoint - normalized_counts;
00045
00046 integeral_term += throttle_setpoint - normalized_counts;
00047
00048 delta_previous = delta_current;
00049 delta_current = proportional_term;
00050 derivative_term = delta_previous - delta_current;
00051
00052 throttle_pulse_duration = ceilf((float)THROTTLE_ZERO + K_P
00053 * (float)proportional_term + K_I * (float)integeral_term
00054 + K_D * (float)derivative_term);
00055 if(throttle_pulse_duration > FULL_FORWARD)
00056 throttle_pulse_duration = FULL_FORWARD;
00057 else if(throttle_pulse_duration < FULL_REVERSE)
00058 throttle_pulse_duration = FULL_REVERSE;
00059 if(throttle_setpoint == 0) throttle_pulse_duration = THROTTLE_ZERO;
00060 PULSOUT (throttle_pulsout_pin, throttle_pulse_duration);
00061 return throttle_pulse_duration;
00062 }
00063
00064 void resetIntegralAndDerivative() {
00065 integeral_term = 0;
00066 derivative_term = 0;
00067 }
00068
00069 int pulsoutThrottleNoPID(int throttle_setpoint) {
00070 static int throttle_pulse_duration = 0;
00071
00072 if(throttle_pulse_duration - THROTTLE_ZERO != throttle_setpoint)
00073 throttle_pulse_duration += ceilf(((float)throttle_setpoint
00074 - (float)(throttle_pulse_duration - THROTTLE_ZERO)) / 2.0);
00075
00076 PULSOUT (throttle_pulsout_pin, throttle_pulse_duration);
00077 return throttle_pulse_duration;
00078 }
00079
00080
00081
00082 static short steering_pulsin_pin, throttle_pulsin_pin;
00083 static int steering_pulsin_duration = 0, throttle_pulsin_duration = 0;
00084
00085
00086 void initializeSteeringAndThrottleIn(int steering_pin, int throttle_pin) {
00087
00088 steering_pulsin_pin = steering_pin;
00089 throttle_pulsin_pin = throttle_pin;
00090 INPUT(steering_pulsin_pin);
00091 INPUT(throttle_pulsin_pin);
00092 }
00093
00094
00095
00096 static short sample_flag = 0;
00097 int checkForSteeringAndThrottle() {
00098 if(sample_flag && !IN(steering_pulsin_pin)) {
00099
00100 sample_flag = 0;
00101 steering_pulsin_duration = PULSIN(steering_pulsin_pin, 1);
00102 } else if(!IN(throttle_pulsin_pin)) {
00103
00104 sample_flag = 1;
00105 throttle_pulsin_duration = PULSIN(throttle_pulsin_pin, 1);
00106 }
00107 return (steering_pulsin_duration != 0 && throttle_pulsin_duration != 0);
00108 }
00109
00110
00111 int getSteeringPulsinDuration() {
00112
00113 if(steering_pulsin_duration < FULL_RIGHT)
00114 steering_pulsin_duration = FULL_RIGHT;
00115 else if(steering_pulsin_duration > FULL_LEFT)
00116 steering_pulsin_duration = FULL_LEFT;
00117 return steering_pulsin_duration - STEERING_ZERO;
00118 }
00119
00120 int getThrottlePulsinDuration() {
00121 if(throttle_pulsin_duration < FULL_REVERSE)
00122 throttle_pulsin_duration = FULL_REVERSE;
00123 else if(throttle_pulsin_duration > FULL_FORWARD)
00124 throttle_pulsin_duration = FULL_FORWARD;
00125 return throttle_pulsin_duration - THROTTLE_ZERO;
00126 }