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@51:6a84fbc404c8, 2017-04-27 (annotated)

Committer:
lh14g13
Date:
Thu Apr 27 15:58:18 2017 +0000
Revision:
51:6a84fbc404c8
Parent:
50:1cfe1f975b0c
Child:
54:78854efeb1df
fixed error

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
lh14g13 46:6806ea59ffed 8
FatCookies 17:6ae90788cc2b 9 #define CLOSE_CAMERA TFC_LineScanImage0
FatCookies 17:6ae90788cc2b 10 #define LOOKAHEAD_CAMERA TFC_LineScanImage1
FatCookies 17:6ae90788cc2b 11
FatCookies 40:10e8e80af7da 12 #define CHANGE_PID 'A'
FatCookies 40:10e8e80af7da 13 #define SEND_SPEEDS 'B'
FatCookies 40:10e8e80af7da 14 #define STOP 'C'
FatCookies 40:10e8e80af7da 15 #define START 'D'
FatCookies 40:10e8e80af7da 16 #define STATUS_STRING 'E'
FatCookies 40:10e8e80af7da 17 #define MAX_SPEED 'F'
FatCookies 40:10e8e80af7da 18 #define TURNING_RATIO 'G'
FatCookies 40:10e8e80af7da 19 #define SEND_LINE 'H'
FatCookies 40:10e8e80af7da 20 #define RECIEVE_THRESHOLD_LINE 'I'
FatCookies 40:10e8e80af7da 21 #define BATTERY_LEVEL 'J'
FatCookies 40:10e8e80af7da 22 #define SWITCH_CAM 'K'
FatCookies 40:10e8e80af7da 23 #define CHANGE_ED 'L'
FatCookies 40:10e8e80af7da 24
FatCookies 17:6ae90788cc2b 25
FatCookies 17:6ae90788cc2b 26 #define ALIGN_SERVO TFC_InitServos(0.00052,0.00122,0.02)
FatCookies 12:da96e2f87465 27
FatCookies 12:da96e2f87465 28 typedef struct {
FatCookies 12:da96e2f87465 29 float Kp;
FatCookies 12:da96e2f87465 30 float Ki;
FatCookies 12:da96e2f87465 31 float Kd;
FatCookies 12:da96e2f87465 32 float dt;
FatCookies 12:da96e2f87465 33 float p_error;
FatCookies 12:da96e2f87465 34 float pid_error;
FatCookies 12:da96e2f87465 35 float integral;
FatCookies 13:4e77264f254a 36
FatCookies 13:4e77264f254a 37 //Measured value is a float between -1.0 and 1.0 (from left to right)
FatCookies 13:4e77264f254a 38 float measured_value;
FatCookies 13:4e77264f254a 39
FatCookies 13:4e77264f254a 40 //Desired value is always 0.0 (as in, car is in the middle of the road)
FatCookies 13:4e77264f254a 41 float desired_value;
FatCookies 13:4e77264f254a 42 float derivative;
FatCookies 12:da96e2f87465 43 float output;
FatCookies 12:da96e2f87465 44 } pid_instance;
FatCookies 12:da96e2f87465 45
FatCookies 43:649473c5a12b 46 extern void writeFloat(float f);
FatCookies 41:d74878640739 47 extern float readFloat();
FatCookies 17:6ae90788cc2b 48 extern inline void handleComms();
FatCookies 17:6ae90788cc2b 49 extern void sendString(const char *format, ...);
FatCookies 17:6ae90788cc2b 50 extern inline void sendImage();
FatCookies 17:6ae90788cc2b 51 extern inline void sendSpeeds();
FatCookies 17:6ae90788cc2b 52 extern void sendBattery();
FatCookies 17:6ae90788cc2b 53
FatCookies 40:10e8e80af7da 54 extern void buttonStartup();
FatCookies 17:6ae90788cc2b 55 extern void initVariables();
FatCookies 17:6ae90788cc2b 56 extern void initPID(pid_instance* pid, float Kp, float Ki, float Kd);
FatCookies 17:6ae90788cc2b 57 extern inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
FatCookies 17:6ae90788cc2b 58 extern inline void PIDController();
FatCookies 17:6ae90788cc2b 59 extern void handlePID(pid_instance *pid);
FatCookies 17:6ae90788cc2b 60 extern inline void handleStartStop();
FatCookies 17:6ae90788cc2b 61 extern inline void handleCornering();
FatCookies 17:6ae90788cc2b 62 extern inline float getLineEntropy();
FatCookies 17:6ae90788cc2b 63 extern inline void initSpeedSensors();
FatCookies 17:6ae90788cc2b 64
FatCookies 43:649473c5a12b 65 // Startup buttons
FatCookies 43:649473c5a12b 66 bool aDown = false;
FatCookies 43:649473c5a12b 67 bool bDown = false;
FatCookies 43:649473c5a12b 68 int b_pressed = 0;
FatCookies 43:649473c5a12b 69 uint8_t led_values[] = {1,3,7,15};
FatCookies 43:649473c5a12b 70
FatCookies 17:6ae90788cc2b 71
FatCookies 17:6ae90788cc2b 72 //Woo global variables!
FatCookies 17:6ae90788cc2b 73 bool onTrack;
FatCookies 17:6ae90788cc2b 74 char curr_line[128];
FatCookies 17:6ae90788cc2b 75 uint8_t curr_left;
FatCookies 17:6ae90788cc2b 76 uint8_t curr_right;
FatCookies 17:6ae90788cc2b 77 uint8_t right;
FatCookies 17:6ae90788cc2b 78 uint8_t left;
FatCookies 17:6ae90788cc2b 79 uint8_t farRight;
FatCookies 17:6ae90788cc2b 80 uint8_t farLeft;
FatCookies 17:6ae90788cc2b 81 int diff;
FatCookies 17:6ae90788cc2b 82 int prev;
FatCookies 17:6ae90788cc2b 83 int i = 0;
FatCookies 17:6ae90788cc2b 84 float measuredValBuffer[64];
FatCookies 17:6ae90788cc2b 85 uint8_t valBufferIndex;
FatCookies 17:6ae90788cc2b 86
FatCookies 17:6ae90788cc2b 87 //Communication Variables
FatCookies 17:6ae90788cc2b 88 uint8_t sendCam = 0;
FatCookies 17:6ae90788cc2b 89 int frame_counter = 0;
FatCookies 17:6ae90788cc2b 90 char curr_cmd = 0; // Current comms command
FatCookies 17:6ae90788cc2b 91 union {
FatCookies 28:613239f10ba4 92 float _float;
FatCookies 17:6ae90788cc2b 93 unsigned char bytes[4];
FatCookies 27:627d67e3b9b0 94 } byte_float_union;
FatCookies 17:6ae90788cc2b 95
FatCookies 17:6ae90788cc2b 96 //Some PID variables
FatCookies 17:6ae90788cc2b 97 pid_instance servo_pid;
FatCookies 17:6ae90788cc2b 98 pid_instance right_motor_pid;
FatCookies 17:6ae90788cc2b 99 pid_instance left_motor_pid;
FatCookies 17:6ae90788cc2b 100
FatCookies 17:6ae90788cc2b 101 // Maximum car speed (PWM)
FatCookies 17:6ae90788cc2b 102 float speed;
FatCookies 17:6ae90788cc2b 103
lh14g13 38:72a174cccd43 104 // Tuning variable
lh14g13 38:72a174cccd43 105
lh14g13 39:7b28ee39185d 106 float ed_tune;
FatCookies 17:6ae90788cc2b 107 //Hacky start/stop signal detection
FatCookies 17:6ae90788cc2b 108 int startstop;
FatCookies 17:6ae90788cc2b 109 bool seen;
FatCookies 17:6ae90788cc2b 110
FatCookies 17:6ae90788cc2b 111 // Speed sensors
FatCookies 17:6ae90788cc2b 112 volatile float Time_L,Time_R;
FatCookies 17:6ae90788cc2b 113 float wL, wR;
FatCookies 20:ed954836d028 114 float prevL, prevR;
FatCookies 17:6ae90788cc2b 115 void GetTime_L();
FatCookies 17:6ae90788cc2b 116 void GetTime_R();
FatCookies 17:6ae90788cc2b 117
FatCookies 17:6ae90788cc2b 118 // Turning detection
FatCookies 17:6ae90788cc2b 119 int turning;
FatCookies 17:6ae90788cc2b 120 int keepTurning;
FatCookies 17:6ae90788cc2b 121 bool slow;
lh14g13 18:0095a3a8f8e4 122
lh14g13 18:0095a3a8f8e4 123
lh14g13 18:0095a3a8f8e4 124 //testing functions for the car
lh14g13 18:0095a3a8f8e4 125 int lapTime();
lh14g13 18:0095a3a8f8e4 126 float testSpeed(float speed);
lh14g13 18:0095a3a8f8e4 127 void endTest();
lh14g13 18:0095a3a8f8e4 128
lh14g13 18:0095a3a8f8e4 129 float oldTime;
lh14g13 18:0095a3a8f8e4 130 int lapNo;
lh14g13 51:6a84fbc404c8 131
FatCookies 45:3435bdd2d487 132 //Accelerometer:
FatCookies 45:3435bdd2d487 133 float checkAcc();
lh14g13 48:daa2a1900ada 134 float accTheshold=0.4;
lh14g13 51:6a84fbc404c8 135 bool torque = 1;
lh14g13 47:6a58dcef714f 136 float man_tuner = 1.0f ;
FatCookies 45:3435bdd2d487 137 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
FatCookies 45:3435bdd2d487 138 PinName const SDA = PTE25;
FatCookies 45:3435bdd2d487 139 PinName const SCL = PTE24;
FatCookies 45:3435bdd2d487 140 #elif defined (TARGET_KL05Z)
FatCookies 45:3435bdd2d487 141 PinName const SDA = PTB4;
FatCookies 45:3435bdd2d487 142 PinName const SCL = PTB3;
FatCookies 45:3435bdd2d487 143 #elif defined (TARGET_K20D50M)
FatCookies 45:3435bdd2d487 144 PinName const SDA = PTB1;
FatCookies 45:3435bdd2d487 145 PinName const SCL = PTB0;
FatCookies 45:3435bdd2d487 146 #else
FatCookies 45:3435bdd2d487 147 #error TARGET NOT DEFINED
FatCookies 45:3435bdd2d487 148 #endif
FatCookies 45:3435bdd2d487 149
FatCookies 45:3435bdd2d487 150 #define MMA8451_I2C_ADDRESS (0x1d<<1)