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@40:10e8e80af7da, 2017-01-21 (annotated)

Committer:
FatCookies
Date:
Sat Jan 21 11:59:20 2017 +0000
Revision:
40:10e8e80af7da
Parent:
39:7b28ee39185d
Child:
41:d74878640739
cleaned up handle comms function with command labels

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 17:6ae90788cc2b 45 extern inline void handleComms();
FatCookies 17:6ae90788cc2b 46 extern void sendString(const char *format, ...);
FatCookies 17:6ae90788cc2b 47 extern inline void sendImage();
FatCookies 17:6ae90788cc2b 48 extern inline void sendSpeeds();
FatCookies 17:6ae90788cc2b 49 extern void sendBattery();
FatCookies 17:6ae90788cc2b 50
FatCookies 40:10e8e80af7da 51 extern void buttonStartup();
FatCookies 17:6ae90788cc2b 52 extern void initVariables();
FatCookies 17:6ae90788cc2b 53 extern void initPID(pid_instance* pid, float Kp, float Ki, float Kd);
FatCookies 17:6ae90788cc2b 54 extern inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
FatCookies 17:6ae90788cc2b 55 extern inline void PIDController();
FatCookies 17:6ae90788cc2b 56 extern void handlePID(pid_instance *pid);
FatCookies 17:6ae90788cc2b 57 extern inline void handleStartStop();
FatCookies 17:6ae90788cc2b 58 extern inline void handleCornering();
FatCookies 17:6ae90788cc2b 59 extern inline float getLineEntropy();
FatCookies 17:6ae90788cc2b 60 extern inline void initSpeedSensors();
FatCookies 17:6ae90788cc2b 61
FatCookies 17:6ae90788cc2b 62
FatCookies 17:6ae90788cc2b 63 //Woo global variables!
FatCookies 17:6ae90788cc2b 64 bool onTrack;
FatCookies 17:6ae90788cc2b 65 char curr_line[128];
FatCookies 17:6ae90788cc2b 66 uint8_t curr_left;
FatCookies 17:6ae90788cc2b 67 uint8_t curr_right;
FatCookies 17:6ae90788cc2b 68 uint8_t right;
FatCookies 17:6ae90788cc2b 69 uint8_t left;
FatCookies 17:6ae90788cc2b 70 uint8_t farRight;
FatCookies 17:6ae90788cc2b 71 uint8_t farLeft;
FatCookies 17:6ae90788cc2b 72 int diff;
FatCookies 17:6ae90788cc2b 73 int prev;
FatCookies 17:6ae90788cc2b 74 int i = 0;
FatCookies 17:6ae90788cc2b 75 float measuredValBuffer[64];
FatCookies 17:6ae90788cc2b 76 uint8_t valBufferIndex;
FatCookies 17:6ae90788cc2b 77
FatCookies 17:6ae90788cc2b 78 //Communication Variables
FatCookies 17:6ae90788cc2b 79 uint8_t sendCam = 0;
FatCookies 17:6ae90788cc2b 80 int frame_counter = 0;
FatCookies 17:6ae90788cc2b 81 char curr_cmd = 0; // Current comms command
FatCookies 17:6ae90788cc2b 82 union {
FatCookies 28:613239f10ba4 83 float _float;
FatCookies 17:6ae90788cc2b 84 unsigned char bytes[4];
FatCookies 27:627d67e3b9b0 85 } byte_float_union;
FatCookies 17:6ae90788cc2b 86
FatCookies 17:6ae90788cc2b 87 //Some PID variables
FatCookies 17:6ae90788cc2b 88 pid_instance servo_pid;
FatCookies 17:6ae90788cc2b 89 pid_instance right_motor_pid;
FatCookies 17:6ae90788cc2b 90 pid_instance left_motor_pid;
FatCookies 17:6ae90788cc2b 91
FatCookies 17:6ae90788cc2b 92 // Maximum car speed (PWM)
FatCookies 17:6ae90788cc2b 93 float speed;
FatCookies 17:6ae90788cc2b 94
lh14g13 38:72a174cccd43 95 // Tuning variable
lh14g13 38:72a174cccd43 96
lh14g13 39:7b28ee39185d 97 float ed_tune;
FatCookies 17:6ae90788cc2b 98 //Hacky start/stop signal detection
FatCookies 17:6ae90788cc2b 99 int startstop;
FatCookies 17:6ae90788cc2b 100 bool seen;
FatCookies 17:6ae90788cc2b 101
FatCookies 17:6ae90788cc2b 102 // Speed sensors
FatCookies 17:6ae90788cc2b 103 volatile float Time_L,Time_R;
FatCookies 17:6ae90788cc2b 104 float wL, wR;
FatCookies 20:ed954836d028 105 float prevL, prevR;
FatCookies 17:6ae90788cc2b 106 void GetTime_L();
FatCookies 17:6ae90788cc2b 107 void GetTime_R();
FatCookies 17:6ae90788cc2b 108
FatCookies 17:6ae90788cc2b 109 // Turning detection
FatCookies 17:6ae90788cc2b 110 int turning;
FatCookies 17:6ae90788cc2b 111 int keepTurning;
FatCookies 17:6ae90788cc2b 112 bool slow;
lh14g13 18:0095a3a8f8e4 113
lh14g13 18:0095a3a8f8e4 114
lh14g13 18:0095a3a8f8e4 115 //testing functions for the car
lh14g13 18:0095a3a8f8e4 116 int lapTime();
lh14g13 18:0095a3a8f8e4 117 float testSpeed(float speed);
lh14g13 18:0095a3a8f8e4 118 void endTest();
lh14g13 18:0095a3a8f8e4 119
lh14g13 18:0095a3a8f8e4 120 float oldTime;
lh14g13 18:0095a3a8f8e4 121 int lapNo;