File content as of revision 91:fdadfd883825:

#ifndef GLOBALS_H
#define GLOBALS_H

//#define TEAM_RED
#define TEAM_BLUE

#include "mbed.h"

const float KALMAN_PREDICT_PERIOD = 0.05; //seconds

#define ENABLE_GLOBAL_ENCODERS

const float ENCODER_M_PER_TICK = 1.0f/1198.0f;
const float ENCODER_WHEELBASE = 0.068;
const float TURRET_FWD_PLACEMENT = -0.042;

//Robot movement constants
const float fwdvarperunit = 0.0001; //1 std dev = 7% //TODO: measrue this!!
const float varperang = 0.0003; //around 3 degree stddev per 180 turn //TODO: measrue this!!
const float xyvarpertime = 0.000025; //(very poorly) accounts for hitting things
const float angvarpertime = 0.0001;  

const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this
const float MOTOR_MAX_POWER = 0.5f;

/*
PINOUT Sensors
5:  RF:SDI
6 SDO
7 SCK
8 NCS
9 NIRQ
10-15 6 echo pins
16 trig
17 IRin
18-20 unused
21 stepper step
22-27 unused
28 Serial TX
29-30 unused


PINOUT Main
5: Lower arm servo
6: Upper arm servo

14: Serial RX
15: Cake distance sensor
16: Fwd distance sensor

20: color sensor in
21-24: Motors PWM IN 1-4
25-26: Encoders 
27-28: Encoders 
29: Color sensor RED LED
30: Color sensor BLUE LED

*/

const PinName P_SERVO_LOWER_ARM = p25;
const PinName P_SERVO_UPPER_ARM = p26;

const PinName P_SERIAL_RX       = p14;
const PinName P_DISTANCE_SENSOR = p15;
const PinName P_FWD_DISTANCE_SENSOR = p16;

const PinName P_COLOR_SENSOR_IN_UPPER = p20;
const PinName P_COLOR_SENSOR_IN_LOWER = p19;

const PinName P_MOT_LEFT_A     = p22;
const PinName P_MOT_LEFT_B     = p21;
const PinName P_MOT_RIGHT_A      = p24;
const PinName P_MOT_RIGHT_B      = p23;

const PinName P_ENC_RIGHT_A     = p28;//p26;
const PinName P_ENC_RIGHT_B     = p27;//p25;
const PinName P_ENC_LEFT_A      = p29;//p27;
const PinName P_ENC_LEFT_B      = p30;//p28;

const PinName P_COLOR_SENSOR_RED_UPPER = p13;//p29;
const PinName P_COLOR_SENSOR_BLUE_UPPER = p12;//p30;
const PinName P_COLOR_SENSOR_RED_LOWER = p11;
const PinName P_COLOR_SENSOR_BLUE_LOWER = p10;

const PinName P_START_CORD = p17;

const PinName P_BALLOON = p8;



//a type which is a pointer to a rtos thread function
typedef void (*tfuncptr_t)(void const *argument);

//Solving for sonar bias is done by entering the following into wolfram alpha
//(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
//where a, b, c are the measured distances, and f is the bias

//const float sonartimebias = 0; // TODO: measure and stick in the coprocessor code

struct pos {
    float x;
    float y;
};

extern pos beaconpos[3];

const float PI = 3.14159265359;

typedef struct Waypoint
{
    float x;
    float y;
    float theta;
    float pos_threshold;
    float angle_threshold;
    float angle_exponent; //temp hack
} Waypoint;

typedef struct State 
{
    float x;
    float y;
    float theta;
} State;

#endif //GLOBALS_H