![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
globals.h@89:dfe8c2ec5b88, 2013-04-16 (annotated)
- Committer:
- madcowswe
- Date:
- Tue Apr 16 13:19:17 2013 +0000
- Revision:
- 89:dfe8c2ec5b88
- Parent:
- 87:272a7129b04b
blue works
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
madcowswe | 16:52250d8d8fce | 1 | |
madcowswe | 20:70d651156779 | 2 | #ifndef GLOBALS_H |
madcowswe | 20:70d651156779 | 3 | #define GLOBALS_H |
madcowswe | 20:70d651156779 | 4 | |
madcowswe | 89:dfe8c2ec5b88 | 5 | //#define TEAM_RED |
madcowswe | 89:dfe8c2ec5b88 | 6 | #define TEAM_BLUE |
madcowswe | 72:7996aa8286ae | 7 | |
madcowswe | 20:70d651156779 | 8 | #include "mbed.h" |
madcowswe | 20:70d651156779 | 9 | |
madcowswe | 20:70d651156779 | 10 | const float KALMAN_PREDICT_PERIOD = 0.05; //seconds |
madcowswe | 20:70d651156779 | 11 | |
madcowswe | 20:70d651156779 | 12 | #define ENABLE_GLOBAL_ENCODERS |
madcowswe | 20:70d651156779 | 13 | |
madcowswe | 62:78d99b781f02 | 14 | const float ENCODER_M_PER_TICK = 1.0f/1198.0f; |
madcowswe | 26:7cb3a21d9a2e | 15 | const float ENCODER_WHEELBASE = 0.068; |
madcowswe | 31:ada943ecaceb | 16 | const float TURRET_FWD_PLACEMENT = -0.042; |
madcowswe | 20:70d651156779 | 17 | |
madcowswe | 20:70d651156779 | 18 | //Robot movement constants |
madcowswe | 79:0d3140048526 | 19 | const float fwdvarperunit = 0.0001; //1 std dev = 7% //TODO: measrue this!! |
madcowswe | 79:0d3140048526 | 20 | const float varperang = 0.0003; //around 3 degree stddev per 180 turn //TODO: measrue this!! |
madcowswe | 79:0d3140048526 | 21 | const float xyvarpertime = 0.000025; //(very poorly) accounts for hitting things |
madcowswe | 79:0d3140048526 | 22 | const float angvarpertime = 0.0001; |
madcowswe | 22:6e3218cf75f8 | 23 | |
madcowswe | 25:b16f1045108f | 24 | const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this |
madcowswe | 33:a49197572737 | 25 | const float MOTOR_MAX_POWER = 0.5f; |
madcowswe | 20:70d651156779 | 26 | |
madcowswe | 20:70d651156779 | 27 | /* |
madcowswe | 20:70d651156779 | 28 | PINOUT Sensors |
madcowswe | 20:70d651156779 | 29 | 5: RF:SDI |
madcowswe | 20:70d651156779 | 30 | 6 SDO |
madcowswe | 20:70d651156779 | 31 | 7 SCK |
madcowswe | 20:70d651156779 | 32 | 8 NCS |
madcowswe | 20:70d651156779 | 33 | 9 NIRQ |
madcowswe | 20:70d651156779 | 34 | 10-15 6 echo pins |
madcowswe | 20:70d651156779 | 35 | 16 trig |
madcowswe | 20:70d651156779 | 36 | 17 IRin |
madcowswe | 20:70d651156779 | 37 | 18-20 unused |
madcowswe | 20:70d651156779 | 38 | 21 stepper step |
madcowswe | 20:70d651156779 | 39 | 22-27 unused |
madcowswe | 20:70d651156779 | 40 | 28 Serial TX |
madcowswe | 20:70d651156779 | 41 | 29-30 unused |
madcowswe | 20:70d651156779 | 42 | |
madcowswe | 20:70d651156779 | 43 | |
madcowswe | 20:70d651156779 | 44 | PINOUT Main |
madcowswe | 20:70d651156779 | 45 | 5: Lower arm servo |
madcowswe | 20:70d651156779 | 46 | 6: Upper arm servo |
madcowswe | 20:70d651156779 | 47 | |
madcowswe | 20:70d651156779 | 48 | 14: Serial RX |
madcowswe | 20:70d651156779 | 49 | 15: Cake distance sensor |
madcowswe | 20:70d651156779 | 50 | 16: Fwd distance sensor |
madcowswe | 20:70d651156779 | 51 | |
madcowswe | 20:70d651156779 | 52 | 20: color sensor in |
madcowswe | 20:70d651156779 | 53 | 21-24: Motors PWM IN 1-4 |
madcowswe | 20:70d651156779 | 54 | 25-26: Encoders |
madcowswe | 20:70d651156779 | 55 | 27-28: Encoders |
madcowswe | 20:70d651156779 | 56 | 29: Color sensor RED LED |
madcowswe | 20:70d651156779 | 57 | 30: Color sensor BLUE LED |
madcowswe | 20:70d651156779 | 58 | |
madcowswe | 20:70d651156779 | 59 | */ |
madcowswe | 20:70d651156779 | 60 | |
rsavitski | 58:ff2121f34e3b | 61 | const PinName P_SERVO_LOWER_ARM = p25; |
rsavitski | 58:ff2121f34e3b | 62 | const PinName P_SERVO_UPPER_ARM = p26; |
madcowswe | 20:70d651156779 | 63 | |
madcowswe | 20:70d651156779 | 64 | const PinName P_SERIAL_RX = p14; |
madcowswe | 20:70d651156779 | 65 | const PinName P_DISTANCE_SENSOR = p15; |
madcowswe | 31:ada943ecaceb | 66 | const PinName P_FWD_DISTANCE_SENSOR = p16; |
madcowswe | 20:70d651156779 | 67 | |
xiaxia686 | 43:c592bf6a6a2d | 68 | const PinName P_COLOR_SENSOR_IN_UPPER = p20; |
xiaxia686 | 43:c592bf6a6a2d | 69 | const PinName P_COLOR_SENSOR_IN_LOWER = p19; |
madcowswe | 20:70d651156779 | 70 | |
madcowswe | 60:5058465904e0 | 71 | const PinName P_MOT_LEFT_A = p22; |
madcowswe | 60:5058465904e0 | 72 | const PinName P_MOT_LEFT_B = p21; |
madcowswe | 60:5058465904e0 | 73 | const PinName P_MOT_RIGHT_A = p24; |
madcowswe | 60:5058465904e0 | 74 | const PinName P_MOT_RIGHT_B = p23; |
madcowswe | 20:70d651156779 | 75 | |
rsavitski | 58:ff2121f34e3b | 76 | const PinName P_ENC_RIGHT_A = p28;//p26; |
rsavitski | 58:ff2121f34e3b | 77 | const PinName P_ENC_RIGHT_B = p27;//p25; |
rsavitski | 58:ff2121f34e3b | 78 | const PinName P_ENC_LEFT_A = p29;//p27; |
rsavitski | 58:ff2121f34e3b | 79 | const PinName P_ENC_LEFT_B = p30;//p28; |
madcowswe | 20:70d651156779 | 80 | |
rsavitski | 58:ff2121f34e3b | 81 | const PinName P_COLOR_SENSOR_RED_UPPER = p13;//p29; |
rsavitski | 58:ff2121f34e3b | 82 | const PinName P_COLOR_SENSOR_BLUE_UPPER = p12;//p30; |
xiaxia686 | 43:c592bf6a6a2d | 83 | const PinName P_COLOR_SENSOR_RED_LOWER = p11; |
xiaxia686 | 43:c592bf6a6a2d | 84 | const PinName P_COLOR_SENSOR_BLUE_LOWER = p10; |
madcowswe | 20:70d651156779 | 85 | |
rsavitski | 77:8d83a0c00e66 | 86 | const PinName P_START_CORD = p17; |
madcowswe | 71:eb1956c2d316 | 87 | |
madcowswe | 84:00c799fd10a7 | 88 | const PinName P_BALLOON = p8; |
madcowswe | 84:00c799fd10a7 | 89 | |
madcowswe | 20:70d651156779 | 90 | |
madcowswe | 20:70d651156779 | 91 | |
madcowswe | 20:70d651156779 | 92 | //a type which is a pointer to a rtos thread function |
madcowswe | 20:70d651156779 | 93 | typedef void (*tfuncptr_t)(void const *argument); |
madcowswe | 16:52250d8d8fce | 94 | |
madcowswe | 16:52250d8d8fce | 95 | //Solving for sonar bias is done by entering the following into wolfram alpha |
madcowswe | 16:52250d8d8fce | 96 | //(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 |
madcowswe | 16:52250d8d8fce | 97 | //where a, b, c are the measured distances, and f is the bias |
madcowswe | 16:52250d8d8fce | 98 | |
madcowswe | 19:4b993a9a156e | 99 | //const float sonartimebias = 0; // TODO: measure and stick in the coprocessor code |
madcowswe | 17:6263e90bf3ba | 100 | |
madcowswe | 17:6263e90bf3ba | 101 | struct pos { |
madcowswe | 17:6263e90bf3ba | 102 | float x; |
madcowswe | 17:6263e90bf3ba | 103 | float y; |
madcowswe | 17:6263e90bf3ba | 104 | }; |
madcowswe | 17:6263e90bf3ba | 105 | |
madcowswe | 19:4b993a9a156e | 106 | extern pos beaconpos[3]; |
madcowswe | 19:4b993a9a156e | 107 | |
madcowswe | 20:70d651156779 | 108 | const float PI = 3.14159265359; |
madcowswe | 20:70d651156779 | 109 | |
rsavitski | 24:50805ef8c499 | 110 | typedef struct Waypoint |
rsavitski | 24:50805ef8c499 | 111 | { |
rsavitski | 24:50805ef8c499 | 112 | float x; |
rsavitski | 24:50805ef8c499 | 113 | float y; |
rsavitski | 24:50805ef8c499 | 114 | float theta; |
rsavitski | 24:50805ef8c499 | 115 | float pos_threshold; |
rsavitski | 24:50805ef8c499 | 116 | float angle_threshold; |
rsavitski | 77:8d83a0c00e66 | 117 | float angle_exponent; //temp hack |
rsavitski | 24:50805ef8c499 | 118 | } Waypoint; |
rsavitski | 24:50805ef8c499 | 119 | |
madcowswe | 33:a49197572737 | 120 | typedef struct State |
madcowswe | 33:a49197572737 | 121 | { |
madcowswe | 33:a49197572737 | 122 | float x; |
madcowswe | 33:a49197572737 | 123 | float y; |
madcowswe | 33:a49197572737 | 124 | float theta; |
madcowswe | 33:a49197572737 | 125 | } State; |
madcowswe | 33:a49197572737 | 126 | |
madcowswe | 20:70d651156779 | 127 | #endif //GLOBALS_H |