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@45:3435bdd2d487, 2017-03-23 (annotated)

Committer:
FatCookies
Date:
Thu Mar 23 09:36:39 2017 +0000
Revision:
45:3435bdd2d487
Parent:
43:649473c5a12b
Child:
46:6806ea59ffed
testing out accelerometer data for slope detection

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 45:3435bdd2d487 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 43:649473c5a12b 45 extern void writeFloat(float f);
FatCookies 41:d74878640739 46 extern float readFloat();
FatCookies 17:6ae90788cc2b 47 extern inline void handleComms();
FatCookies 17:6ae90788cc2b 48 extern void sendString(const char *format, ...);
FatCookies 17:6ae90788cc2b 49 extern inline void sendImage();
FatCookies 17:6ae90788cc2b 50 extern inline void sendSpeeds();
FatCookies 17:6ae90788cc2b 51 extern void sendBattery();
FatCookies 17:6ae90788cc2b 52
FatCookies 40:10e8e80af7da 53 extern void buttonStartup();
FatCookies 17:6ae90788cc2b 54 extern void initVariables();
FatCookies 17:6ae90788cc2b 55 extern void initPID(pid_instance* pid, float Kp, float Ki, float Kd);
FatCookies 17:6ae90788cc2b 56 extern inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
FatCookies 17:6ae90788cc2b 57 extern inline void PIDController();
FatCookies 17:6ae90788cc2b 58 extern void handlePID(pid_instance *pid);
FatCookies 17:6ae90788cc2b 59 extern inline void handleStartStop();
FatCookies 17:6ae90788cc2b 60 extern inline void handleCornering();
FatCookies 17:6ae90788cc2b 61 extern inline float getLineEntropy();
FatCookies 17:6ae90788cc2b 62 extern inline void initSpeedSensors();
FatCookies 17:6ae90788cc2b 63
FatCookies 43:649473c5a12b 64 // Startup buttons
FatCookies 43:649473c5a12b 65 bool aDown = false;
FatCookies 43:649473c5a12b 66 bool bDown = false;
FatCookies 43:649473c5a12b 67 int b_pressed = 0;
FatCookies 43:649473c5a12b 68 uint8_t led_values[] = {1,3,7,15};
FatCookies 43:649473c5a12b 69
FatCookies 17:6ae90788cc2b 70
FatCookies 17:6ae90788cc2b 71 //Woo global variables!
FatCookies 17:6ae90788cc2b 72 bool onTrack;
FatCookies 17:6ae90788cc2b 73 char curr_line[128];
FatCookies 17:6ae90788cc2b 74 uint8_t curr_left;
FatCookies 17:6ae90788cc2b 75 uint8_t curr_right;
FatCookies 17:6ae90788cc2b 76 uint8_t right;
FatCookies 17:6ae90788cc2b 77 uint8_t left;
FatCookies 17:6ae90788cc2b 78 uint8_t farRight;
FatCookies 17:6ae90788cc2b 79 uint8_t farLeft;
FatCookies 17:6ae90788cc2b 80 int diff;
FatCookies 17:6ae90788cc2b 81 int prev;
FatCookies 17:6ae90788cc2b 82 int i = 0;
FatCookies 17:6ae90788cc2b 83 float measuredValBuffer[64];
FatCookies 17:6ae90788cc2b 84 uint8_t valBufferIndex;
FatCookies 17:6ae90788cc2b 85
FatCookies 17:6ae90788cc2b 86 //Communication Variables
FatCookies 17:6ae90788cc2b 87 uint8_t sendCam = 0;
FatCookies 17:6ae90788cc2b 88 int frame_counter = 0;
FatCookies 17:6ae90788cc2b 89 char curr_cmd = 0; // Current comms command
FatCookies 17:6ae90788cc2b 90 union {
FatCookies 28:613239f10ba4 91 float _float;
FatCookies 17:6ae90788cc2b 92 unsigned char bytes[4];
FatCookies 27:627d67e3b9b0 93 } byte_float_union;
FatCookies 17:6ae90788cc2b 94
FatCookies 17:6ae90788cc2b 95 //Some PID variables
FatCookies 17:6ae90788cc2b 96 pid_instance servo_pid;
FatCookies 17:6ae90788cc2b 97 pid_instance right_motor_pid;
FatCookies 17:6ae90788cc2b 98 pid_instance left_motor_pid;
FatCookies 17:6ae90788cc2b 99
FatCookies 17:6ae90788cc2b 100 // Maximum car speed (PWM)
FatCookies 17:6ae90788cc2b 101 float speed;
FatCookies 17:6ae90788cc2b 102
lh14g13 38:72a174cccd43 103 // Tuning variable
lh14g13 38:72a174cccd43 104
lh14g13 39:7b28ee39185d 105 float ed_tune;
FatCookies 17:6ae90788cc2b 106 //Hacky start/stop signal detection
FatCookies 17:6ae90788cc2b 107 int startstop;
FatCookies 17:6ae90788cc2b 108 bool seen;
FatCookies 17:6ae90788cc2b 109
FatCookies 17:6ae90788cc2b 110 // Speed sensors
FatCookies 17:6ae90788cc2b 111 volatile float Time_L,Time_R;
FatCookies 17:6ae90788cc2b 112 float wL, wR;
FatCookies 20:ed954836d028 113 float prevL, prevR;
FatCookies 17:6ae90788cc2b 114 void GetTime_L();
FatCookies 17:6ae90788cc2b 115 void GetTime_R();
FatCookies 17:6ae90788cc2b 116
FatCookies 17:6ae90788cc2b 117 // Turning detection
FatCookies 17:6ae90788cc2b 118 int turning;
FatCookies 17:6ae90788cc2b 119 int keepTurning;
FatCookies 17:6ae90788cc2b 120 bool slow;
lh14g13 18:0095a3a8f8e4 121
lh14g13 18:0095a3a8f8e4 122
lh14g13 18:0095a3a8f8e4 123 //testing functions for the car
lh14g13 18:0095a3a8f8e4 124 int lapTime();
lh14g13 18:0095a3a8f8e4 125 float testSpeed(float speed);
lh14g13 18:0095a3a8f8e4 126 void endTest();
lh14g13 18:0095a3a8f8e4 127
lh14g13 18:0095a3a8f8e4 128 float oldTime;
lh14g13 18:0095a3a8f8e4 129 int lapNo;
FatCookies 45:3435bdd2d487 130
FatCookies 45:3435bdd2d487 131 //Accelerometer:
FatCookies 45:3435bdd2d487 132 float checkAcc();
FatCookies 45:3435bdd2d487 133
FatCookies 45:3435bdd2d487 134 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
FatCookies 45:3435bdd2d487 135 PinName const SDA = PTE25;
FatCookies 45:3435bdd2d487 136 PinName const SCL = PTE24;
FatCookies 45:3435bdd2d487 137 #elif defined (TARGET_KL05Z)
FatCookies 45:3435bdd2d487 138 PinName const SDA = PTB4;
FatCookies 45:3435bdd2d487 139 PinName const SCL = PTB3;
FatCookies 45:3435bdd2d487 140 #elif defined (TARGET_K20D50M)
FatCookies 45:3435bdd2d487 141 PinName const SDA = PTB1;
FatCookies 45:3435bdd2d487 142 PinName const SCL = PTB0;
FatCookies 45:3435bdd2d487 143 #else
FatCookies 45:3435bdd2d487 144 #error TARGET NOT DEFINED
FatCookies 45:3435bdd2d487 145 #endif
FatCookies 45:3435bdd2d487 146
FatCookies 45:3435bdd2d487 147 #define MMA8451_I2C_ADDRESS (0x1d<<1)