File content as of revision 51:6a84fbc404c8:

#define BAUD_RATE 57600
#define CAM_THRESHOLD 100
#define CAM_DIFF 10
#define WHEEL_RADIUS 0.025f
#define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
#define USE_COMMS 1


#define CLOSE_CAMERA TFC_LineScanImage0
#define LOOKAHEAD_CAMERA TFC_LineScanImage1

#define CHANGE_PID 'A' 
#define SEND_SPEEDS 'B' 
#define STOP 'C' 
#define START 'D' 
#define STATUS_STRING 'E' 
#define MAX_SPEED 'F' 
#define TURNING_RATIO 'G' 
#define SEND_LINE 'H' 
#define RECIEVE_THRESHOLD_LINE 'I' 
#define BATTERY_LEVEL 'J' 
#define SWITCH_CAM 'K' 
#define CHANGE_ED 'L'


#define ALIGN_SERVO TFC_InitServos(0.00052,0.00122,0.02)

typedef struct {
    float Kp;
    float Ki;
    float Kd;
    float dt;
    float p_error;
    float pid_error;
    float integral;
        
    //Measured value is a float between -1.0 and 1.0 (from left to right)
    float measured_value;
    
    //Desired value is always 0.0 (as in, car is in the middle of the road)
    float desired_value;
    float derivative;
    float output;
} pid_instance;

extern void writeFloat(float f);
extern float readFloat();
extern inline void handleComms();
extern void sendString(const char *format, ...);
extern inline void sendImage();
extern inline void sendSpeeds();
extern void sendBattery();

extern void buttonStartup();
extern void initVariables();
extern void initPID(pid_instance* pid, float Kp, float Ki, float Kd);
extern inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
extern inline void PIDController();
extern void handlePID(pid_instance *pid);
extern inline void handleStartStop();
extern inline void handleCornering();
extern inline float getLineEntropy();
extern inline void initSpeedSensors();

// Startup buttons
bool aDown = false;
bool bDown = false;
int b_pressed = 0;
uint8_t led_values[] = {1,3,7,15};


//Woo global variables!
bool onTrack;
char curr_line[128];
uint8_t curr_left;
uint8_t curr_right;
uint8_t right;
uint8_t left;
uint8_t farRight;
uint8_t farLeft;
int diff;
int prev;
int i = 0;
float measuredValBuffer[64];
uint8_t valBufferIndex;

//Communication Variables
uint8_t sendCam = 0;
int frame_counter = 0;
char curr_cmd = 0; // Current comms command
union {
    float _float;
    unsigned char bytes[4];
} byte_float_union;

//Some PID variables
pid_instance servo_pid;
pid_instance right_motor_pid;
pid_instance left_motor_pid;

// Maximum car speed (PWM)
float speed;

// Tuning variable

float ed_tune;
//Hacky start/stop signal detection
int startstop;
bool seen;

// Speed sensors
volatile float Time_L,Time_R;
float wL, wR;
float prevL, prevR;
void GetTime_L();
void GetTime_R();

// Turning detection
int turning;
int keepTurning;
bool slow;


//testing functions for the car
int lapTime();
float testSpeed(float speed);
void endTest();

float oldTime;
int lapNo;

//Accelerometer:
float checkAcc();
float accTheshold=0.4;
bool torque = 1;
 float man_tuner = 1.0f ;
#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
  PinName const SDA = PTE25;
  PinName const SCL = PTE24;
#elif defined (TARGET_KL05Z)
  PinName const SDA = PTB4;
  PinName const SCL = PTB3;
#elif defined (TARGET_K20D50M)
  PinName const SDA = PTB1;
  PinName const SCL = PTB0;
#else
  #error TARGET NOT DEFINED
#endif
 
#define MMA8451_I2C_ADDRESS (0x1d<<1)