Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

steering_methods.h@34:cb9a0cec2feb, 2021-11-22 (annotated)

Committer:
aalawfi
Date:
Mon Nov 22 22:27:14 2021 +0000
Revision:
34:cb9a0cec2feb
Parent:
31:d570f957e083 
- 3 Laps, fastest is ~~12.3s

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aalawfi 2:c857935f928e 1 //#include "steering_header.h"
aalawfi 2:c857935f928e 2 /*
aalawfi 2:c857935f928e 3 Include methods and functions
aalawfi 2:c857935f928e 4 */
aalawfi 2:c857935f928e 5
aalawfi 2:c857935f928e 6 // --------- LANDMARK COUNTER --------
aalawfi 2:c857935f928e 7 static int decimal_to_bin[16][BITS] = {
aalawfi 2:c857935f928e 8 {0,0,0,0}, // zero
aalawfi 2:c857935f928e 9 {0,0,0,1}, // one
aalawfi 2:c857935f928e 10 {0,0,1,0}, //two
aalawfi 2:c857935f928e 11 {0,0,1,1}, //three
aalawfi 2:c857935f928e 12 {0,1,0,0}, // four
aalawfi 2:c857935f928e 13 {0,1,0,1}, //five
aalawfi 2:c857935f928e 14 {0,1,1,0}, // six
aalawfi 2:c857935f928e 15 {0,1,1,1}, //seven
aalawfi 2:c857935f928e 16 {1,0,0,0}, //eight
aalawfi 2:c857935f928e 17 {1,0,0,1}, //nine
aalawfi 2:c857935f928e 18 {1,0,1,0}, //ten
aalawfi 2:c857935f928e 19 {1,0,1,1}, //11
aalawfi 2:c857935f928e 20 {1,1,0,0}, //12
aalawfi 2:c857935f928e 21 {1,1,0,1}, //13
aalawfi 2:c857935f928e 22 {1,1,1,0}, //14
aalawfi 2:c857935f928e 23 {1,1,1,1}, //15
aalawfi 2:c857935f928e 24 } ;
aalawfi 2:c857935f928e 25 void turn_seg_off(DigitalOut segment[]){
aalawfi 2:c857935f928e 26 for(int i =0; i<BITS; i++){
aalawfi 2:c857935f928e 27 segment[i] = 0;
aalawfi 2:c857935f928e 28 }
aalawfi 2:c857935f928e 29 }
aalawfi 2:c857935f928e 30 void show_decimal(int number, DigitalOut segment[]) {
aalawfi 2:c857935f928e 31 turn_seg_off(segment);
aalawfi 2:c857935f928e 32 for(int i =0; i < BITS; i++){
aalawfi 2:c857935f928e 33 segment[i] = decimal_to_bin[number][i];
aalawfi 2:c857935f928e 34 };
aalawfi 2:c857935f928e 35 }
aalawfi 2:c857935f928e 36 // ------------ LANDMARK DETECTION ------------
aalawfi 23:4c743746533c 37 Timer land;
aalawfi 25:8bd029d58251 38
quarren42 19:65fecaa2a387 39 volatile bool landmark_triggered=false; // Debugging purposes only
quarren42 19:65fecaa2a387 40 volatile bool restarted = false; // Debugging purposes only
aalawfi 23:4c743746533c 41
aalawfi 2:c857935f928e 42 void landmark_counter(void){
aalawfi 23:4c743746533c 43 land.start();
quarren42 19:65fecaa2a387 44 landmark_triggered=false; // Debugging purposes only
quarren42 19:65fecaa2a387 45 restarted = false; // Debugging purposes only
aalawfi 2:c857935f928e 46 if(counter >= 15)
aalawfi 2:c857935f928e 47 counter =0;
aalawfi 2:c857935f928e 48 turn_seg_off(seg1);
aalawfi 25:8bd029d58251 49 if((right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1) && land.read_us() > 10 )
aalawfi 25:8bd029d58251 50 {
aalawfi 23:4c743746533c 51 landmark_triggered =true; // Debugging purpose only
aalawfi 25:8bd029d58251 52 (counter > 6 ? counter = 7 : counter++);
aalawfi 23:4c743746533c 53 land.stop();
aalawfi 23:4c743746533c 54 }
aalawfi 23:4c743746533c 55
aalawfi 23:4c743746533c 56 // }
aalawfi 31:d570f957e083 57 if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1)
quarren42 19:65fecaa2a387 58 {
aalawfi 25:8bd029d58251 59 lap++;
aalawfi 23:4c743746533c 60 counter = 0;
aalawfi 23:4c743746533c 61 restarted=true;
aalawfi 25:8bd029d58251 62 // Debugging purposes only
quarren42 19:65fecaa2a387 63 }
aalawfi 23:4c743746533c 64 land.stop();
aalawfi 2:c857935f928e 65 show_decimal(counter, seg1);
aalawfi 2:c857935f928e 66 }
aalawfi 2:c857935f928e 67
aalawfi 2:c857935f928e 68
aalawfi 2:c857935f928e 69 float rad2dc (float angle){
aalawfi 2:c857935f928e 70 angle = std::max(MIN_TURN_ANGLE, std::min(angle, MAX_TURN_ANGLE));
aalawfi 2:c857935f928e 71 float x = (angle - MIN_TURN_ANGLE)/(MAX_TURN_ANGLE - MIN_TURN_ANGLE);
aalawfi 2:c857935f928e 72 float dutyCycle = 0.05*x+0.05;
aalawfi 2:c857935f928e 73 return dutyCycle;
aalawfi 2:c857935f928e 74 }
aalawfi 2:c857935f928e 75
aalawfi 2:c857935f928e 76 float current_duty_cycle=0.075;
aalawfi 2:c857935f928e 77 // -------------- PD CONTROLLER --------------
aalawfi 2:c857935f928e 78 // Global declearation
aalawfi 2:c857935f928e 79 float err_prev = 0;
quarren42 6:d2bd68ba99c9 80 float prev_duty = 0;
aalawfi 22:08d30ea47111 81 float errorArea;
aalawfi 22:08d30ea47111 82 float errorAreaPrev =0.0;
aalawfi 25:8bd029d58251 83 float feedback;
aalawfi 25:8bd029d58251 84 float err;
aalawfi 25:8bd029d58251 85 float left_sens; float right_sens;
aalawfi 25:8bd029d58251 86 volatile bool clamp;
aalawfi 28:1c2eb25d624e 87 float OFFSET_ANGLE=0.0; float KICK_ERR; float PREV_KICK_ERR; float KICK_CHNG_ERR;
aalawfi 31:d570f957e083 88 Timer k;
aalawfi 2:c857935f928e 89 void steering_control(void)
aalawfi 2:c857935f928e 90 {
aalawfi 2:c857935f928e 91
aalawfi 25:8bd029d58251 92
aalawfi 2:c857935f928e 93 if(steering_enabled == true ){
aalawfi 2:c857935f928e 94 //Read each sensor and calculate the sensor difference
aalawfi 26:54ce9f642477 95
aalawfi 26:54ce9f642477 96
aalawfi 26:54ce9f642477 97 float sensor_difference = left_distance_sensor.read() -
aalawfi 22:08d30ea47111 98 right_distance_sensor.read();
aalawfi 28:1c2eb25d624e 99
aalawfi 31:d570f957e083 100 REF = 0.0;
aalawfi 31:d570f957e083 101 if(counter ==1 || counter == 2 )
aalawfi 31:d570f957e083 102 REF = 0.01;
aalawfi 31:d570f957e083 103 if(counter == 3)
aalawfi 31:d570f957e083 104 {
aalawfi 31:d570f957e083 105 k.start();
aalawfi 31:d570f957e083 106 REF = -0.01;
aalawfi 31:d570f957e083 107 if(k.read_ms() > 500)
aalawfi 31:d570f957e083 108 REF = 0.0;
aalawfi 31:d570f957e083 109 k.stop();
aalawfi 31:d570f957e083 110 }
aalawfi 31:d570f957e083 111 if(counter == 6 || counter == 7)
aalawfi 31:d570f957e083 112 {
aalawfi 31:d570f957e083 113 REF=0.02;
aalawfi 31:d570f957e083 114
aalawfi 31:d570f957e083 115 }
aalawfi 31:d570f957e083 116
aalawfi 31:d570f957e083 117
aalawfi 31:d570f957e083 118
aalawfi 28:1c2eb25d624e 119 /*if (counter == 1 && lap == 3){
aalawfi 28:1c2eb25d624e 120 KP = 6;
aalawfi 28:1c2eb25d624e 121 KD = 1;
aalawfi 28:1c2eb25d624e 122 KI_STEERINg = 0.8;
aalawfi 28:1c2eb25d624e 123
aalawfi 28:1c2eb25d624e 124 }*/
aalawfi 2:c857935f928e 125 //calculate the feedback term for the controller. distance (meters) from
aalawfi 2:c857935f928e 126 //track to sensor
aalawfi 25:8bd029d58251 127 feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
aalawfi 25:8bd029d58251 128
aalawfi 2:c857935f928e 129 //calculate the error term for the controller. distance (meters) from
aalawfi 2:c857935f928e 130 //desired position to sensor
aalawfi 2:c857935f928e 131 err = REF - feedback; //
aalawfi 28:1c2eb25d624e 132 KICK_ERR = -1 * feedback;
aalawfi 22:08d30ea47111 133 //Area of the error: TimeStep*err (width*length of a rectangle)
aalawfi 26:54ce9f642477 134
aalawfi 27:0fa9d61c5fc6 135 // if(clamp == false)
aalawfi 22:08d30ea47111 136 errorArea= TI_STEERING*err + errorAreaPrev;
aalawfi 25:8bd029d58251 137
aalawfi 2:c857935f928e 138 //Calculate the derivative of the error term for the controller.
aalawfi 2:c857935f928e 139 // e'(t) = (error(t) - error(t-1))/dt
aalawfi 22:08d30ea47111 140 float errChange = (err - err_prev)/(TI_STEERING);
aalawfi 28:1c2eb25d624e 141 KICK_CHNG_ERR = (KICK_ERR - PREV_KICK_ERR)/(TI_STEERING);
aalawfi 2:c857935f928e 142 //Calculate the controller output
aalawfi 2:c857935f928e 143 //Angle in radians
aalawfi 31:d570f957e083 144 float servo_angle = (OFFSET_ANGLE+KP*err + KD*KICK_CHNG_ERR*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
aalawfi 2:c857935f928e 145 //Calculate the duty cycle for the servo motor
aalawfi 2:c857935f928e 146 current_duty_cycle = rad2dc(servo_angle);
aalawfi 24:7bf492bf50f4 147
aalawfi 26:54ce9f642477 148 //--- integral anti-windup ---
aalawfi 25:8bd029d58251 149 if (servo_angle > 0.1 || servo_angle < 0.05){
aalawfi 25:8bd029d58251 150 if (errorArea > 0.0){
aalawfi 25:8bd029d58251 151 clamp = true;
aalawfi 25:8bd029d58251 152 }
aalawfi 25:8bd029d58251 153 } else {
aalawfi 25:8bd029d58251 154 clamp = false;
aalawfi 25:8bd029d58251 155 }
aalawfi 26:54ce9f642477 156
aalawfi 28:1c2eb25d624e 157 float strCheck = rad2dc(0);
aalawfi 28:1c2eb25d624e 158 servo_motor_pwm.write(current_duty_cycle);
aalawfi 26:54ce9f642477 159
aalawfi 28:1c2eb25d624e 160 err_prev = err;
aalawfi 28:1c2eb25d624e 161 PREV_KICK_ERR = KICK_ERR;
aalawfi 28:1c2eb25d624e 162 errorAreaPrev = errorArea;
aalawfi 22:08d30ea47111 163
aalawfi 2:c857935f928e 164 }
aalawfi 9:5320c2dfb913 165 else {
aalawfi 2:c857935f928e 166 current_duty_cycle = rad2dc(M_PI/4);
aalawfi 25:8bd029d58251 167
aalawfi 2:c857935f928e 168 servo_motor_pwm.write(current_duty_cycle);
aalawfi 9:5320c2dfb913 169 };
aalawfi 28:1c2eb25d624e 170
aalawfi 2:c857935f928e 171 }
aalawfi 2:c857935f928e 172
aalawfi 2:c857935f928e 173
aalawfi 2:c857935f928e 174