GDP 4 / Mbed 2 deprecated pid-car-example

car using PID from centre line

Dependencies:   FRDM-TFC mbed CBuffer XBEE mbed_angular_speed motor2 MMA8451Q

Fork of KL25Z_Camera_Test by GDP 4

main.h@41:d74878640739, 2017-01-25 (annotated)

Committer:
FatCookies
Date:
Wed Jan 25 15:46:50 2017 +0000
Revision:
41:d74878640739
Parent:
40:10e8e80af7da
Child:
43:649473c5a12b
readFloat function added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
FatCookies 17:6ae90788cc2b 1 #define BAUD_RATE 57600
FatCookies 17:6ae90788cc2b 2 #define CAM_THRESHOLD 100
FatCookies 17:6ae90788cc2b 3 #define CAM_DIFF 10
FatCookies 17:6ae90788cc2b 4 #define WHEEL_RADIUS 0.025f
FatCookies 17:6ae90788cc2b 5 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
FatCookies 17:6ae90788cc2b 6 #define USE_COMMS 1
FatCookies 17:6ae90788cc2b 7
FatCookies 17:6ae90788cc2b 8 #define CLOSE_CAMERA TFC_LineScanImage0
FatCookies 17:6ae90788cc2b 9 #define LOOKAHEAD_CAMERA TFC_LineScanImage1
FatCookies 17:6ae90788cc2b 10
FatCookies 40:10e8e80af7da 11 #define CHANGE_PID 'A'
FatCookies 40:10e8e80af7da 12 #define SEND_SPEEDS 'B'
FatCookies 40:10e8e80af7da 13 #define STOP 'C'
FatCookies 40:10e8e80af7da 14 #define START 'D'
FatCookies 40:10e8e80af7da 15 #define STATUS_STRING 'E'
FatCookies 40:10e8e80af7da 16 #define MAX_SPEED 'F'
FatCookies 40:10e8e80af7da 17 #define TURNING_RATIO 'G'
FatCookies 40:10e8e80af7da 18 #define SEND_LINE 'H'
FatCookies 40:10e8e80af7da 19 #define RECIEVE_THRESHOLD_LINE 'I'
FatCookies 40:10e8e80af7da 20 #define BATTERY_LEVEL 'J'
FatCookies 40:10e8e80af7da 21 #define SWITCH_CAM 'K'
FatCookies 40:10e8e80af7da 22 #define CHANGE_ED 'L'
FatCookies 40:10e8e80af7da 23
FatCookies 17:6ae90788cc2b 24
FatCookies 17:6ae90788cc2b 25 #define ALIGN_SERVO TFC_InitServos(0.00052,0.00122,0.02)
FatCookies 12:da96e2f87465 26
FatCookies 12:da96e2f87465 27 typedef struct {
FatCookies 12:da96e2f87465 28 float Kp;
FatCookies 12:da96e2f87465 29 float Ki;
FatCookies 12:da96e2f87465 30 float Kd;
FatCookies 12:da96e2f87465 31 float dt;
FatCookies 12:da96e2f87465 32 float p_error;
FatCookies 12:da96e2f87465 33 float pid_error;
FatCookies 12:da96e2f87465 34 float integral;
FatCookies 13:4e77264f254a 35
FatCookies 13:4e77264f254a 36 //Measured value is a float between -1.0 and 1.0 (from left to right)
FatCookies 13:4e77264f254a 37 float measured_value;
FatCookies 13:4e77264f254a 38
FatCookies 13:4e77264f254a 39 //Desired value is always 0.0 (as in, car is in the middle of the road)
FatCookies 13:4e77264f254a 40 float desired_value;
FatCookies 13:4e77264f254a 41 float derivative;
FatCookies 12:da96e2f87465 42 float output;
FatCookies 12:da96e2f87465 43 } pid_instance;
FatCookies 12:da96e2f87465 44
FatCookies 41:d74878640739 45 extern float readFloat();
FatCookies 17:6ae90788cc2b 46 extern inline void handleComms();
FatCookies 17:6ae90788cc2b 47 extern void sendString(const char *format, ...);
FatCookies 17:6ae90788cc2b 48 extern inline void sendImage();
FatCookies 17:6ae90788cc2b 49 extern inline void sendSpeeds();
FatCookies 17:6ae90788cc2b 50 extern void sendBattery();
FatCookies 17:6ae90788cc2b 51
FatCookies 40:10e8e80af7da 52 extern void buttonStartup();
FatCookies 17:6ae90788cc2b 53 extern void initVariables();
FatCookies 17:6ae90788cc2b 54 extern void initPID(pid_instance* pid, float Kp, float Ki, float Kd);
FatCookies 17:6ae90788cc2b 55 extern inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
FatCookies 17:6ae90788cc2b 56 extern inline void PIDController();
FatCookies 17:6ae90788cc2b 57 extern void handlePID(pid_instance *pid);
FatCookies 17:6ae90788cc2b 58 extern inline void handleStartStop();
FatCookies 17:6ae90788cc2b 59 extern inline void handleCornering();
FatCookies 17:6ae90788cc2b 60 extern inline float getLineEntropy();
FatCookies 17:6ae90788cc2b 61 extern inline void initSpeedSensors();
FatCookies 17:6ae90788cc2b 62
FatCookies 17:6ae90788cc2b 63
FatCookies 17:6ae90788cc2b 64 //Woo global variables!
FatCookies 17:6ae90788cc2b 65 bool onTrack;
FatCookies 17:6ae90788cc2b 66 char curr_line[128];
FatCookies 17:6ae90788cc2b 67 uint8_t curr_left;
FatCookies 17:6ae90788cc2b 68 uint8_t curr_right;
FatCookies 17:6ae90788cc2b 69 uint8_t right;
FatCookies 17:6ae90788cc2b 70 uint8_t left;
FatCookies 17:6ae90788cc2b 71 uint8_t farRight;
FatCookies 17:6ae90788cc2b 72 uint8_t farLeft;
FatCookies 17:6ae90788cc2b 73 int diff;
FatCookies 17:6ae90788cc2b 74 int prev;
FatCookies 17:6ae90788cc2b 75 int i = 0;
FatCookies 17:6ae90788cc2b 76 float measuredValBuffer[64];
FatCookies 17:6ae90788cc2b 77 uint8_t valBufferIndex;
FatCookies 17:6ae90788cc2b 78
FatCookies 17:6ae90788cc2b 79 //Communication Variables
FatCookies 17:6ae90788cc2b 80 uint8_t sendCam = 0;
FatCookies 17:6ae90788cc2b 81 int frame_counter = 0;
FatCookies 17:6ae90788cc2b 82 char curr_cmd = 0; // Current comms command
FatCookies 17:6ae90788cc2b 83 union {
FatCookies 28:613239f10ba4 84 float _float;
FatCookies 17:6ae90788cc2b 85 unsigned char bytes[4];
FatCookies 27:627d67e3b9b0 86 } byte_float_union;
FatCookies 17:6ae90788cc2b 87
FatCookies 17:6ae90788cc2b 88 //Some PID variables
FatCookies 17:6ae90788cc2b 89 pid_instance servo_pid;
FatCookies 17:6ae90788cc2b 90 pid_instance right_motor_pid;
FatCookies 17:6ae90788cc2b 91 pid_instance left_motor_pid;
FatCookies 17:6ae90788cc2b 92
FatCookies 17:6ae90788cc2b 93 // Maximum car speed (PWM)
FatCookies 17:6ae90788cc2b 94 float speed;
FatCookies 17:6ae90788cc2b 95
lh14g13 38:72a174cccd43 96 // Tuning variable
lh14g13 38:72a174cccd43 97
lh14g13 39:7b28ee39185d 98 float ed_tune;
FatCookies 17:6ae90788cc2b 99 //Hacky start/stop signal detection
FatCookies 17:6ae90788cc2b 100 int startstop;
FatCookies 17:6ae90788cc2b 101 bool seen;
FatCookies 17:6ae90788cc2b 102
FatCookies 17:6ae90788cc2b 103 // Speed sensors
FatCookies 17:6ae90788cc2b 104 volatile float Time_L,Time_R;
FatCookies 17:6ae90788cc2b 105 float wL, wR;
FatCookies 20:ed954836d028 106 float prevL, prevR;
FatCookies 17:6ae90788cc2b 107 void GetTime_L();
FatCookies 17:6ae90788cc2b 108 void GetTime_R();
FatCookies 17:6ae90788cc2b 109
FatCookies 17:6ae90788cc2b 110 // Turning detection
FatCookies 17:6ae90788cc2b 111 int turning;
FatCookies 17:6ae90788cc2b 112 int keepTurning;
FatCookies 17:6ae90788cc2b 113 bool slow;
lh14g13 18:0095a3a8f8e4 114
lh14g13 18:0095a3a8f8e4 115
lh14g13 18:0095a3a8f8e4 116 //testing functions for the car
lh14g13 18:0095a3a8f8e4 117 int lapTime();
lh14g13 18:0095a3a8f8e4 118 float testSpeed(float speed);
lh14g13 18:0095a3a8f8e4 119 void endTest();
lh14g13 18:0095a3a8f8e4 120
lh14g13 18:0095a3a8f8e4 121 float oldTime;
lh14g13 18:0095a3a8f8e4 122 int lapNo;