Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

steering_methods.h@19:65fecaa2a387, 2021-10-28 (annotated)

Committer:
quarren42
Date:
Thu Oct 28 00:28:29 2021 +0000
Revision:
19:65fecaa2a387
Parent:
18:831c1e03d83e
Child:
20:7dcdadbd7bc0
most recent, code works??

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 2:c857935f928e 37 int counter = 0;
quarren42 19:65fecaa2a387 38 volatile bool landmark_triggered=false; // Debugging purposes only
quarren42 19:65fecaa2a387 39 volatile bool restarted = false; // Debugging purposes only
aalawfi 2:c857935f928e 40 void landmark_counter(void){
quarren42 19:65fecaa2a387 41 landmark_triggered=false; // Debugging purposes only
quarren42 19:65fecaa2a387 42 restarted = false; // Debugging purposes only
aalawfi 2:c857935f928e 43 if(counter >= 15)
aalawfi 2:c857935f928e 44 counter =0;
aalawfi 2:c857935f928e 45 turn_seg_off(seg1);
aalawfi 2:c857935f928e 46 if(right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1 )
quarren42 19:65fecaa2a387 47 {
quarren42 19:65fecaa2a387 48 landmark_triggered =true; // Debugging purpose only
aalawfi 2:c857935f928e 49 counter++ ;
quarren42 19:65fecaa2a387 50
quarren42 19:65fecaa2a387 51 }
aalawfi 2:c857935f928e 52 if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1 )
quarren42 19:65fecaa2a387 53 {
quarren42 19:65fecaa2a387 54 counter = 0;
quarren42 19:65fecaa2a387 55 restarted=true; // Debugging purposes only
quarren42 19:65fecaa2a387 56 }
aalawfi 13:0091da3021df 57
aalawfi 2:c857935f928e 58 show_decimal(counter, seg1);
aalawfi 2:c857935f928e 59 }
aalawfi 2:c857935f928e 60
aalawfi 2:c857935f928e 61
aalawfi 2:c857935f928e 62 float rad2dc (float angle){
aalawfi 2:c857935f928e 63 angle = std::max(MIN_TURN_ANGLE, std::min(angle, MAX_TURN_ANGLE));
aalawfi 2:c857935f928e 64 float x = (angle - MIN_TURN_ANGLE)/(MAX_TURN_ANGLE - MIN_TURN_ANGLE);
aalawfi 2:c857935f928e 65 float dutyCycle = 0.05*x+0.05;
aalawfi 2:c857935f928e 66 return dutyCycle;
aalawfi 2:c857935f928e 67 }
aalawfi 2:c857935f928e 68
aalawfi 2:c857935f928e 69 float current_duty_cycle=0.075;
aalawfi 2:c857935f928e 70 // -------------- PD CONTROLLER --------------
aalawfi 2:c857935f928e 71 // Global declearation
aalawfi 2:c857935f928e 72 float err_prev = 0;
quarren42 6:d2bd68ba99c9 73 float prev_duty = 0;
aalawfi 2:c857935f928e 74 void steering_control(void)
aalawfi 2:c857935f928e 75 {
aalawfi 2:c857935f928e 76
aalawfi 2:c857935f928e 77 float err;
aalawfi 2:c857935f928e 78 if(steering_enabled == true ){
aalawfi 2:c857935f928e 79 //Read each sensor and calculate the sensor difference
quarren42 19:65fecaa2a387 80 float sensor_difference = left_distance_sensor.read() -
quarren42 19:65fecaa2a387 81 right_distance_sensor.read()*1.4;
aalawfi 2:c857935f928e 82
aalawfi 2:c857935f928e 83 //calculate the feedback term for the controller. distance (meters) from
aalawfi 2:c857935f928e 84 //track to sensor
aalawfi 2:c857935f928e 85 float feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
aalawfi 2:c857935f928e 86
aalawfi 2:c857935f928e 87 //calculate the error term for the controller. distance (meters) from
aalawfi 2:c857935f928e 88 //desired position to sensor
aalawfi 2:c857935f928e 89 err = REF - feedback; //
aalawfi 2:c857935f928e 90
aalawfi 2:c857935f928e 91 //Calculate the derivative of the error term for the controller.
aalawfi 2:c857935f928e 92 // e'(t) = (error(t) - error(t-1))/dt
quarren42 18:831c1e03d83e 93 float errChange = (err - err_prev)/(0.02);
aalawfi 2:c857935f928e 94
aalawfi 2:c857935f928e 95 //Calculate the controller output
aalawfi 2:c857935f928e 96 //Angle in radians
aalawfi 2:c857935f928e 97 float servo_angle = (KP*err + KD*errChange*TIME_DERIVATIVE)/10;
aalawfi 2:c857935f928e 98 //Calculate the duty cycle for the servo motor
aalawfi 2:c857935f928e 99 current_duty_cycle = rad2dc(servo_angle);
quarren42 19:65fecaa2a387 100 if( abs(current_duty_cycle - prev_duty) >= 0.0000103){
aalawfi 2:c857935f928e 101 servo_motor_pwm.write(current_duty_cycle);
aalawfi 9:5320c2dfb913 102 };
aalawfi 2:c857935f928e 103 }
aalawfi 9:5320c2dfb913 104 else {
aalawfi 2:c857935f928e 105 current_duty_cycle = rad2dc(M_PI/4);
aalawfi 2:c857935f928e 106 servo_motor_pwm.write(current_duty_cycle);
aalawfi 9:5320c2dfb913 107 };
aalawfi 2:c857935f928e 108 //Save the error for the next calculation.
aalawfi 2:c857935f928e 109 prev_duty = current_duty_cycle;
aalawfi 2:c857935f928e 110 err_prev = err;
aalawfi 2:c857935f928e 111
aalawfi 2:c857935f928e 112 }
aalawfi 2:c857935f928e 113
aalawfi 2:c857935f928e 114
aalawfi 2:c857935f928e 115