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@27:627d67e3b9b0, 2016-12-15 (annotated)

Committer:
FatCookies
Date:
Thu Dec 15 16:39:32 2016 +0000
Revision:
27:627d67e3b9b0
Parent:
20:ed954836d028
Child:
28:613239f10ba4
changed slip detection (no unintended acceleration), refactored code aka finally got rid of thing

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 17:6ae90788cc2b 11
FatCookies 17:6ae90788cc2b 12 #define ALIGN_SERVO TFC_InitServos(0.00052,0.00122,0.02)
FatCookies 12:da96e2f87465 13
FatCookies 12:da96e2f87465 14 typedef struct {
FatCookies 12:da96e2f87465 15 float Kp;
FatCookies 12:da96e2f87465 16 float Ki;
FatCookies 12:da96e2f87465 17 float Kd;
FatCookies 12:da96e2f87465 18 float dt;
FatCookies 12:da96e2f87465 19 float p_error;
FatCookies 12:da96e2f87465 20 float pid_error;
FatCookies 12:da96e2f87465 21 float integral;
FatCookies 13:4e77264f254a 22
FatCookies 13:4e77264f254a 23 //Measured value is a float between -1.0 and 1.0 (from left to right)
FatCookies 13:4e77264f254a 24 float measured_value;
FatCookies 13:4e77264f254a 25
FatCookies 13:4e77264f254a 26 //Desired value is always 0.0 (as in, car is in the middle of the road)
FatCookies 13:4e77264f254a 27 float desired_value;
FatCookies 13:4e77264f254a 28 float derivative;
FatCookies 12:da96e2f87465 29 float output;
FatCookies 12:da96e2f87465 30 } pid_instance;
FatCookies 12:da96e2f87465 31
FatCookies 17:6ae90788cc2b 32 extern inline void handleComms();
FatCookies 17:6ae90788cc2b 33 extern void sendString(const char *format, ...);
FatCookies 17:6ae90788cc2b 34 extern inline void sendImage();
FatCookies 17:6ae90788cc2b 35 extern inline void sendSpeeds();
FatCookies 17:6ae90788cc2b 36 extern void sendBattery();
FatCookies 17:6ae90788cc2b 37
FatCookies 17:6ae90788cc2b 38 extern void initVariables();
FatCookies 17:6ae90788cc2b 39 extern void initPID(pid_instance* pid, float Kp, float Ki, float Kd);
FatCookies 17:6ae90788cc2b 40 extern inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
FatCookies 17:6ae90788cc2b 41 extern inline void PIDController();
FatCookies 17:6ae90788cc2b 42 extern void handlePID(pid_instance *pid);
FatCookies 17:6ae90788cc2b 43 extern inline void handleStartStop();
FatCookies 17:6ae90788cc2b 44 extern inline void handleCornering();
FatCookies 17:6ae90788cc2b 45 extern inline float getLineEntropy();
FatCookies 17:6ae90788cc2b 46 extern inline void initSpeedSensors();
FatCookies 17:6ae90788cc2b 47
FatCookies 17:6ae90788cc2b 48
FatCookies 17:6ae90788cc2b 49 //Woo global variables!
FatCookies 17:6ae90788cc2b 50 bool onTrack;
FatCookies 17:6ae90788cc2b 51 char curr_line[128];
FatCookies 17:6ae90788cc2b 52 uint8_t curr_left;
FatCookies 17:6ae90788cc2b 53 uint8_t curr_right;
FatCookies 17:6ae90788cc2b 54 uint8_t right;
FatCookies 17:6ae90788cc2b 55 uint8_t left;
FatCookies 17:6ae90788cc2b 56 uint8_t farRight;
FatCookies 17:6ae90788cc2b 57 uint8_t farLeft;
FatCookies 17:6ae90788cc2b 58 int diff;
FatCookies 17:6ae90788cc2b 59 int prev;
FatCookies 17:6ae90788cc2b 60 int i = 0;
FatCookies 17:6ae90788cc2b 61 float measuredValBuffer[64];
FatCookies 17:6ae90788cc2b 62 uint8_t valBufferIndex;
FatCookies 17:6ae90788cc2b 63
FatCookies 17:6ae90788cc2b 64 //Communication Variables
FatCookies 17:6ae90788cc2b 65 uint8_t sendCam = 0;
FatCookies 17:6ae90788cc2b 66 int frame_counter = 0;
FatCookies 17:6ae90788cc2b 67 char curr_cmd = 0; // Current comms command
FatCookies 17:6ae90788cc2b 68 union {
FatCookies 17:6ae90788cc2b 69 float a;
FatCookies 17:6ae90788cc2b 70 unsigned char bytes[4];
FatCookies 27:627d67e3b9b0 71 } byte_float_union;
FatCookies 17:6ae90788cc2b 72
FatCookies 17:6ae90788cc2b 73 //Some PID variables
FatCookies 17:6ae90788cc2b 74 pid_instance servo_pid;
FatCookies 17:6ae90788cc2b 75 pid_instance right_motor_pid;
FatCookies 17:6ae90788cc2b 76 pid_instance left_motor_pid;
FatCookies 17:6ae90788cc2b 77
FatCookies 17:6ae90788cc2b 78 // Maximum car speed (PWM)
FatCookies 17:6ae90788cc2b 79 float speed;
FatCookies 17:6ae90788cc2b 80
FatCookies 17:6ae90788cc2b 81 //Hacky start/stop signal detection
FatCookies 17:6ae90788cc2b 82 int startstop;
FatCookies 17:6ae90788cc2b 83 bool seen;
FatCookies 17:6ae90788cc2b 84
FatCookies 17:6ae90788cc2b 85 // Speed sensors
FatCookies 17:6ae90788cc2b 86 volatile float Time_L,Time_R;
FatCookies 17:6ae90788cc2b 87 float wL, wR;
FatCookies 20:ed954836d028 88 float prevL, prevR;
FatCookies 17:6ae90788cc2b 89 void GetTime_L();
FatCookies 17:6ae90788cc2b 90 void GetTime_R();
FatCookies 17:6ae90788cc2b 91
FatCookies 17:6ae90788cc2b 92 // Turning detection
FatCookies 17:6ae90788cc2b 93 int turning;
FatCookies 17:6ae90788cc2b 94 int keepTurning;
FatCookies 17:6ae90788cc2b 95 bool slow;
lh14g13 18:0095a3a8f8e4 96
lh14g13 18:0095a3a8f8e4 97
lh14g13 18:0095a3a8f8e4 98 //testing functions for the car
lh14g13 18:0095a3a8f8e4 99 int lapTime();
lh14g13 18:0095a3a8f8e4 100 float testSpeed(float speed);
lh14g13 18:0095a3a8f8e4 101 void endTest();
lh14g13 18:0095a3a8f8e4 102
lh14g13 18:0095a3a8f8e4 103 float oldTime;
lh14g13 18:0095a3a8f8e4 104 int lapNo;