Last update drone code.
Dependencies: EthernetNetIf mbed ADXL345_I2C ITG3200 L3G4200D IMUfilter
main.cpp@0:d96713a4739f, 2012-06-01 (annotated)
- Committer:
- SED9008
- Date:
- Fri Jun 01 08:17:40 2012 +0000
- Revision:
- 0:d96713a4739f
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
SED9008 | 0:d96713a4739f | 1 | #include "mbed.h" |
SED9008 | 0:d96713a4739f | 2 | #include "EthernetNetIf.h" |
SED9008 | 0:d96713a4739f | 3 | #include "TCPSocket.h" |
SED9008 | 0:d96713a4739f | 4 | #include "ADXL345_I2C.h" |
SED9008 | 0:d96713a4739f | 5 | #include "ITG3200.h" |
SED9008 | 0:d96713a4739f | 6 | #include "IMUfilter.h" |
SED9008 | 0:d96713a4739f | 7 | |
SED9008 | 0:d96713a4739f | 8 | #define TCP_LISTENING_PORT 1337 |
SED9008 | 0:d96713a4739f | 9 | //Gravity at Earth's surface in m/s/s |
SED9008 | 0:d96713a4739f | 10 | #define g0 9.812865328 |
SED9008 | 0:d96713a4739f | 11 | //Number of samples to average. |
SED9008 | 0:d96713a4739f | 12 | #define SAMPLES 4 |
SED9008 | 0:d96713a4739f | 13 | //Number of samples to be averaged for a null bias calculation |
SED9008 | 0:d96713a4739f | 14 | //during calibration. |
SED9008 | 0:d96713a4739f | 15 | #define CALIBRATION_SAMPLES 128 |
SED9008 | 0:d96713a4739f | 16 | //Convert from radians to degrees. |
SED9008 | 0:d96713a4739f | 17 | #define toDegrees(x) (x * 57.2957795) |
SED9008 | 0:d96713a4739f | 18 | //Convert from degrees to radians. |
SED9008 | 0:d96713a4739f | 19 | #define toRadians(x) (x * 0.01745329252) |
SED9008 | 0:d96713a4739f | 20 | //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). |
SED9008 | 0:d96713a4739f | 21 | #define GYROSCOPE_GAIN (1 / 14.375) |
SED9008 | 0:d96713a4739f | 22 | //Full scale resolution on the ADXL345 is 4mg/LSB. |
SED9008 | 0:d96713a4739f | 23 | #define ACCELEROMETER_GAIN (0.004 * g0) |
SED9008 | 0:d96713a4739f | 24 | //Sampling gyroscope at 200Hz. |
SED9008 | 0:d96713a4739f | 25 | #define GYRO_RATE 0.002 |
SED9008 | 0:d96713a4739f | 26 | //Sampling accelerometer at 200Hz. |
SED9008 | 0:d96713a4739f | 27 | #define ACC_RATE 0.002 |
SED9008 | 0:d96713a4739f | 28 | //Updating filter at 100Hz. |
SED9008 | 0:d96713a4739f | 29 | #define FILTER_RATE 0.01 |
SED9008 | 0:d96713a4739f | 30 | #define CORRECTION_MAGNITUDE 50 |
SED9008 | 0:d96713a4739f | 31 | |
SED9008 | 0:d96713a4739f | 32 | |
SED9008 | 0:d96713a4739f | 33 | //At rest the gyroscope is centred around 0 and goes between about |
SED9008 | 0:d96713a4739f | 34 | //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly |
SED9008 | 0:d96713a4739f | 35 | //5/15 = 0.3 degrees/sec. |
SED9008 | 0:d96713a4739f | 36 | IMUfilter imuFilter(FILTER_RATE, 0.3); |
SED9008 | 0:d96713a4739f | 37 | ADXL345_I2C accelerometer(p9, p10); |
SED9008 | 0:d96713a4739f | 38 | I2C i2c(p9, p10); // sda, scl |
SED9008 | 0:d96713a4739f | 39 | ITG3200 gyroscope(p9, p10); |
SED9008 | 0:d96713a4739f | 40 | Ticker accelerometerTicker; |
SED9008 | 0:d96713a4739f | 41 | Ticker gyroscopeTicker; |
SED9008 | 0:d96713a4739f | 42 | Ticker filterTicker; |
SED9008 | 0:d96713a4739f | 43 | Ticker dataTicker; |
SED9008 | 0:d96713a4739f | 44 | Ticker algorithmTicker; |
SED9008 | 0:d96713a4739f | 45 | DigitalOut led1(LED1, "led1"); |
SED9008 | 0:d96713a4739f | 46 | DigitalOut led2(LED2, "led2"); |
SED9008 | 0:d96713a4739f | 47 | DigitalOut led4(LED4, "led4"); |
SED9008 | 0:d96713a4739f | 48 | PwmOut m1(p21); |
SED9008 | 0:d96713a4739f | 49 | PwmOut m2(p22); |
SED9008 | 0:d96713a4739f | 50 | PwmOut m3(p23); |
SED9008 | 0:d96713a4739f | 51 | PwmOut m4(p24); |
SED9008 | 0:d96713a4739f | 52 | |
SED9008 | 0:d96713a4739f | 53 | // EthernetNetIf eth; |
SED9008 | 0:d96713a4739f | 54 | EthernetNetIf eth( |
SED9008 | 0:d96713a4739f | 55 | IpAddr(192,168,0,25), //IP Address |
SED9008 | 0:d96713a4739f | 56 | IpAddr(255,255,255,0), //Network Mask |
SED9008 | 0:d96713a4739f | 57 | IpAddr(192,168,0,1), //Gateway |
SED9008 | 0:d96713a4739f | 58 | IpAddr(192,168,0,1) //DNS |
SED9008 | 0:d96713a4739f | 59 | ); |
SED9008 | 0:d96713a4739f | 60 | |
SED9008 | 0:d96713a4739f | 61 | TCPSocket ListeningSock; |
SED9008 | 0:d96713a4739f | 62 | TCPSocket* pConnectedSock; // for ConnectedSock |
SED9008 | 0:d96713a4739f | 63 | Host client; |
SED9008 | 0:d96713a4739f | 64 | TCPSocketErr err; |
SED9008 | 0:d96713a4739f | 65 | |
SED9008 | 0:d96713a4739f | 66 | int calibrate = 0, |
SED9008 | 0:d96713a4739f | 67 | calibrated = 0, |
SED9008 | 0:d96713a4739f | 68 | start = 0, |
SED9008 | 0:d96713a4739f | 69 | started = 0, |
SED9008 | 0:d96713a4739f | 70 | alg_enable = 0; |
SED9008 | 0:d96713a4739f | 71 | |
SED9008 | 0:d96713a4739f | 72 | double a_motor_1 = 0, |
SED9008 | 0:d96713a4739f | 73 | a_motor_2 = 0, |
SED9008 | 0:d96713a4739f | 74 | a_motor_3 = 0, |
SED9008 | 0:d96713a4739f | 75 | a_motor_4 = 0; |
SED9008 | 0:d96713a4739f | 76 | |
SED9008 | 0:d96713a4739f | 77 | float m1_set = 0.001, |
SED9008 | 0:d96713a4739f | 78 | m2_set = 0.001, |
SED9008 | 0:d96713a4739f | 79 | m3_set = 0.001, |
SED9008 | 0:d96713a4739f | 80 | m4_set = 0.001, |
SED9008 | 0:d96713a4739f | 81 | thrust_m1 = 3, |
SED9008 | 0:d96713a4739f | 82 | thrust_m2 = 3, |
SED9008 | 0:d96713a4739f | 83 | thrust_m3 = 3, |
SED9008 | 0:d96713a4739f | 84 | thrust_m4 = 3; |
SED9008 | 0:d96713a4739f | 85 | |
SED9008 | 0:d96713a4739f | 86 | char data[128]; |
SED9008 | 0:d96713a4739f | 87 | //Offsets for the gyroscope. |
SED9008 | 0:d96713a4739f | 88 | //The readings we take when the gyroscope is stationary won't be 0, so we'll |
SED9008 | 0:d96713a4739f | 89 | //average a set of readings we do get when the gyroscope is stationary and |
SED9008 | 0:d96713a4739f | 90 | //take those away from subsequent readings to ensure the gyroscope is offset |
SED9008 | 0:d96713a4739f | 91 | //or "biased" to 0. |
SED9008 | 0:d96713a4739f | 92 | double w_xBias; |
SED9008 | 0:d96713a4739f | 93 | double w_yBias; |
SED9008 | 0:d96713a4739f | 94 | double w_zBias; |
SED9008 | 0:d96713a4739f | 95 | |
SED9008 | 0:d96713a4739f | 96 | //Offsets for the accelerometer. |
SED9008 | 0:d96713a4739f | 97 | //Same as with the gyroscope. |
SED9008 | 0:d96713a4739f | 98 | double a_xBias; |
SED9008 | 0:d96713a4739f | 99 | double a_yBias; |
SED9008 | 0:d96713a4739f | 100 | double a_zBias; |
SED9008 | 0:d96713a4739f | 101 | |
SED9008 | 0:d96713a4739f | 102 | //Accumulators used for oversampling and then averaging. |
SED9008 | 0:d96713a4739f | 103 | volatile double a_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 104 | volatile double a_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 105 | volatile double a_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 106 | volatile double w_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 107 | volatile double w_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 108 | volatile double w_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 109 | |
SED9008 | 0:d96713a4739f | 110 | //Accelerometer and gyroscope readings for x, y, z axes. |
SED9008 | 0:d96713a4739f | 111 | volatile double a_x; |
SED9008 | 0:d96713a4739f | 112 | volatile double a_y; |
SED9008 | 0:d96713a4739f | 113 | volatile double a_z; |
SED9008 | 0:d96713a4739f | 114 | volatile double w_x; |
SED9008 | 0:d96713a4739f | 115 | volatile double w_y; |
SED9008 | 0:d96713a4739f | 116 | volatile double w_z; |
SED9008 | 0:d96713a4739f | 117 | |
SED9008 | 0:d96713a4739f | 118 | //Buffer for accelerometer readings. |
SED9008 | 0:d96713a4739f | 119 | int readings[3]; |
SED9008 | 0:d96713a4739f | 120 | //Number of accelerometer samples we're on. |
SED9008 | 0:d96713a4739f | 121 | int accelerometerSamples = 0; |
SED9008 | 0:d96713a4739f | 122 | //Number of gyroscope samples we're on. |
SED9008 | 0:d96713a4739f | 123 | int gyroscopeSamples = 0; |
SED9008 | 0:d96713a4739f | 124 | |
SED9008 | 0:d96713a4739f | 125 | void onConnectedTCPSocketEvent(TCPSocketEvent e); |
SED9008 | 0:d96713a4739f | 126 | void onListeningTCPSocketEvent(TCPSocketEvent e); |
SED9008 | 0:d96713a4739f | 127 | void tcp_send(const char*); |
SED9008 | 0:d96713a4739f | 128 | //Set up the ADXL345 appropriately. |
SED9008 | 0:d96713a4739f | 129 | void initializeAcceleromter(void); |
SED9008 | 0:d96713a4739f | 130 | //Calculate the null bias. |
SED9008 | 0:d96713a4739f | 131 | void calibrateAccelerometer(void); |
SED9008 | 0:d96713a4739f | 132 | //Take a set of samples and average them. |
SED9008 | 0:d96713a4739f | 133 | void sampleAccelerometer(void); |
SED9008 | 0:d96713a4739f | 134 | //Set up the ITG3200 appropriately. |
SED9008 | 0:d96713a4739f | 135 | void initializeGyroscope(void); |
SED9008 | 0:d96713a4739f | 136 | //Calculate the null bias. |
SED9008 | 0:d96713a4739f | 137 | void calibrateGyroscope(void); |
SED9008 | 0:d96713a4739f | 138 | //Take a set of samples and average them. |
SED9008 | 0:d96713a4739f | 139 | void sampleGyroscope(void); |
SED9008 | 0:d96713a4739f | 140 | //Update the filter and calculate the Euler angles. |
SED9008 | 0:d96713a4739f | 141 | void filter(void); |
SED9008 | 0:d96713a4739f | 142 | void getI2CAddress(); |
SED9008 | 0:d96713a4739f | 143 | |
SED9008 | 0:d96713a4739f | 144 | void initializeAccelerometer(void) { |
SED9008 | 0:d96713a4739f | 145 | |
SED9008 | 0:d96713a4739f | 146 | //Go into standby mode to configure the device. |
SED9008 | 0:d96713a4739f | 147 | accelerometer.setPowerControl(0x00); |
SED9008 | 0:d96713a4739f | 148 | //Full resolution, +/-16g, 4mg/LSB. |
SED9008 | 0:d96713a4739f | 149 | accelerometer.setDataFormatControl(0x0B); |
SED9008 | 0:d96713a4739f | 150 | //200Hz data rate. |
SED9008 | 0:d96713a4739f | 151 | accelerometer.setDataRate(ADXL345_200HZ); |
SED9008 | 0:d96713a4739f | 152 | //Measurement mode. |
SED9008 | 0:d96713a4739f | 153 | accelerometer.setPowerControl(0x08); |
SED9008 | 0:d96713a4739f | 154 | //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf |
SED9008 | 0:d96713a4739f | 155 | wait_ms(22); |
SED9008 | 0:d96713a4739f | 156 | |
SED9008 | 0:d96713a4739f | 157 | } |
SED9008 | 0:d96713a4739f | 158 | |
SED9008 | 0:d96713a4739f | 159 | void sampleAccelerometer(void) { |
SED9008 | 0:d96713a4739f | 160 | |
SED9008 | 0:d96713a4739f | 161 | //Have we taken enough samples? |
SED9008 | 0:d96713a4739f | 162 | if (accelerometerSamples == SAMPLES) { |
SED9008 | 0:d96713a4739f | 163 | |
SED9008 | 0:d96713a4739f | 164 | //Average the samples, remove the bias, and calculate the acceleration |
SED9008 | 0:d96713a4739f | 165 | //in m/s/s. |
SED9008 | 0:d96713a4739f | 166 | a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; |
SED9008 | 0:d96713a4739f | 167 | a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; |
SED9008 | 0:d96713a4739f | 168 | a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; |
SED9008 | 0:d96713a4739f | 169 | |
SED9008 | 0:d96713a4739f | 170 | a_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 171 | a_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 172 | a_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 173 | accelerometerSamples = 0; |
SED9008 | 0:d96713a4739f | 174 | |
SED9008 | 0:d96713a4739f | 175 | } else { |
SED9008 | 0:d96713a4739f | 176 | //Take another sample. |
SED9008 | 0:d96713a4739f | 177 | accelerometer.getOutput(readings); |
SED9008 | 0:d96713a4739f | 178 | |
SED9008 | 0:d96713a4739f | 179 | a_xAccumulator += (int16_t) readings[0]; |
SED9008 | 0:d96713a4739f | 180 | a_yAccumulator += (int16_t) readings[1]; |
SED9008 | 0:d96713a4739f | 181 | a_zAccumulator += (int16_t) readings[2]; |
SED9008 | 0:d96713a4739f | 182 | |
SED9008 | 0:d96713a4739f | 183 | accelerometerSamples++; |
SED9008 | 0:d96713a4739f | 184 | } |
SED9008 | 0:d96713a4739f | 185 | } |
SED9008 | 0:d96713a4739f | 186 | |
SED9008 | 0:d96713a4739f | 187 | void initializeGyroscope(void) { |
SED9008 | 0:d96713a4739f | 188 | |
SED9008 | 0:d96713a4739f | 189 | //Low pass filter bandwidth of 42Hz. |
SED9008 | 0:d96713a4739f | 190 | gyroscope.setLpBandwidth(LPFBW_42HZ); |
SED9008 | 0:d96713a4739f | 191 | //Internal sample rate of 200Hz. (1kHz / 5). |
SED9008 | 0:d96713a4739f | 192 | gyroscope.setSampleRateDivider(4); |
SED9008 | 0:d96713a4739f | 193 | |
SED9008 | 0:d96713a4739f | 194 | } |
SED9008 | 0:d96713a4739f | 195 | |
SED9008 | 0:d96713a4739f | 196 | void calibrateGyroscope(void) { |
SED9008 | 0:d96713a4739f | 197 | |
SED9008 | 0:d96713a4739f | 198 | w_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 199 | w_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 200 | w_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 201 | |
SED9008 | 0:d96713a4739f | 202 | //Take a number of readings and average them |
SED9008 | 0:d96713a4739f | 203 | //to calculate the gyroscope bias offset. |
SED9008 | 0:d96713a4739f | 204 | for (int i = 0; i < CALIBRATION_SAMPLES; i++) { |
SED9008 | 0:d96713a4739f | 205 | |
SED9008 | 0:d96713a4739f | 206 | w_xAccumulator += gyroscope.getGyroX(); |
SED9008 | 0:d96713a4739f | 207 | w_yAccumulator += gyroscope.getGyroY(); |
SED9008 | 0:d96713a4739f | 208 | w_zAccumulator += gyroscope.getGyroZ(); |
SED9008 | 0:d96713a4739f | 209 | wait(GYRO_RATE); |
SED9008 | 0:d96713a4739f | 210 | |
SED9008 | 0:d96713a4739f | 211 | } |
SED9008 | 0:d96713a4739f | 212 | |
SED9008 | 0:d96713a4739f | 213 | //Average the samples. |
SED9008 | 0:d96713a4739f | 214 | w_xAccumulator /= CALIBRATION_SAMPLES; |
SED9008 | 0:d96713a4739f | 215 | w_yAccumulator /= CALIBRATION_SAMPLES; |
SED9008 | 0:d96713a4739f | 216 | w_zAccumulator /= CALIBRATION_SAMPLES; |
SED9008 | 0:d96713a4739f | 217 | |
SED9008 | 0:d96713a4739f | 218 | w_xBias = w_xAccumulator; |
SED9008 | 0:d96713a4739f | 219 | w_yBias = w_yAccumulator; |
SED9008 | 0:d96713a4739f | 220 | w_zBias = w_zAccumulator; |
SED9008 | 0:d96713a4739f | 221 | |
SED9008 | 0:d96713a4739f | 222 | w_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 223 | w_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 224 | w_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 225 | |
SED9008 | 0:d96713a4739f | 226 | } |
SED9008 | 0:d96713a4739f | 227 | |
SED9008 | 0:d96713a4739f | 228 | void sampleGyroscope(void) { |
SED9008 | 0:d96713a4739f | 229 | |
SED9008 | 0:d96713a4739f | 230 | //Have we taken enough samples? |
SED9008 | 0:d96713a4739f | 231 | if (gyroscopeSamples == SAMPLES) { |
SED9008 | 0:d96713a4739f | 232 | |
SED9008 | 0:d96713a4739f | 233 | //Average the samples, remove the bias, and calculate the angular |
SED9008 | 0:d96713a4739f | 234 | //velocity in rad/s. |
SED9008 | 0:d96713a4739f | 235 | w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); |
SED9008 | 0:d96713a4739f | 236 | w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); |
SED9008 | 0:d96713a4739f | 237 | w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); |
SED9008 | 0:d96713a4739f | 238 | |
SED9008 | 0:d96713a4739f | 239 | w_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 240 | w_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 241 | w_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 242 | gyroscopeSamples = 0; |
SED9008 | 0:d96713a4739f | 243 | |
SED9008 | 0:d96713a4739f | 244 | } else { |
SED9008 | 0:d96713a4739f | 245 | //Take another sample. |
SED9008 | 0:d96713a4739f | 246 | w_xAccumulator += gyroscope.getGyroX(); |
SED9008 | 0:d96713a4739f | 247 | w_yAccumulator += gyroscope.getGyroY(); |
SED9008 | 0:d96713a4739f | 248 | w_zAccumulator += gyroscope.getGyroZ(); |
SED9008 | 0:d96713a4739f | 249 | |
SED9008 | 0:d96713a4739f | 250 | gyroscopeSamples++; |
SED9008 | 0:d96713a4739f | 251 | |
SED9008 | 0:d96713a4739f | 252 | } |
SED9008 | 0:d96713a4739f | 253 | |
SED9008 | 0:d96713a4739f | 254 | } |
SED9008 | 0:d96713a4739f | 255 | |
SED9008 | 0:d96713a4739f | 256 | void filter(void) { |
SED9008 | 0:d96713a4739f | 257 | //Update the filter variables. |
SED9008 | 0:d96713a4739f | 258 | imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); |
SED9008 | 0:d96713a4739f | 259 | //Calculate the new Euler angles. |
SED9008 | 0:d96713a4739f | 260 | imuFilter.computeEuler(); |
SED9008 | 0:d96713a4739f | 261 | alg_enable = 1; |
SED9008 | 0:d96713a4739f | 262 | } |
SED9008 | 0:d96713a4739f | 263 | |
SED9008 | 0:d96713a4739f | 264 | |
SED9008 | 0:d96713a4739f | 265 | void algorithm(void) |
SED9008 | 0:d96713a4739f | 266 | { |
SED9008 | 0:d96713a4739f | 267 | if(alg_enable){ |
SED9008 | 0:d96713a4739f | 268 | |
SED9008 | 0:d96713a4739f | 269 | int temp_x = toDegrees(imuFilter.getRoll()); |
SED9008 | 0:d96713a4739f | 270 | int temp_y = toDegrees(imuFilter.getPitch()); |
SED9008 | 0:d96713a4739f | 271 | |
SED9008 | 0:d96713a4739f | 272 | double x = temp_x; |
SED9008 | 0:d96713a4739f | 273 | double y = temp_y; |
SED9008 | 0:d96713a4739f | 274 | |
SED9008 | 0:d96713a4739f | 275 | a_motor_1 = thrust_m1 + ((((float)x/-2)-((float)y/2))/CORRECTION_MAGNITUDE); |
SED9008 | 0:d96713a4739f | 276 | thrust_m1 += ((((float)x/-2)-((float)y/2))/CORRECTION_MAGNITUDE)/20; |
SED9008 | 0:d96713a4739f | 277 | |
SED9008 | 0:d96713a4739f | 278 | if(a_motor_1 < 0.1){ |
SED9008 | 0:d96713a4739f | 279 | m1_set = 0.001175; |
SED9008 | 0:d96713a4739f | 280 | a_motor_1 = 0.999; |
SED9008 | 0:d96713a4739f | 281 | } |
SED9008 | 0:d96713a4739f | 282 | else{m1_set = (a_motor_1+117.5)/100000;} |
SED9008 | 0:d96713a4739f | 283 | |
SED9008 | 0:d96713a4739f | 284 | |
SED9008 | 0:d96713a4739f | 285 | a_motor_2 = thrust_m2 + ((((float)x/-2)+((float)y/2))/CORRECTION_MAGNITUDE); |
SED9008 | 0:d96713a4739f | 286 | thrust_m2 += ((((float)x/-2)+((float)y/2))/CORRECTION_MAGNITUDE)/20; |
SED9008 | 0:d96713a4739f | 287 | if(a_motor_2 < 0.1){ |
SED9008 | 0:d96713a4739f | 288 | m2_set = 0.001175; |
SED9008 | 0:d96713a4739f | 289 | a_motor_2 = 0.999; |
SED9008 | 0:d96713a4739f | 290 | } |
SED9008 | 0:d96713a4739f | 291 | else{m2_set = (a_motor_2+117.5)/100000;} |
SED9008 | 0:d96713a4739f | 292 | |
SED9008 | 0:d96713a4739f | 293 | a_motor_3 = thrust_m3 + ((((float)x/2)+((float)y/2))/CORRECTION_MAGNITUDE); |
SED9008 | 0:d96713a4739f | 294 | thrust_m3 += ((((float)x/2)+((float)y/2))/CORRECTION_MAGNITUDE)/20; |
SED9008 | 0:d96713a4739f | 295 | if(a_motor_3 < 0.1){ |
SED9008 | 0:d96713a4739f | 296 | m3_set = 0.001175; |
SED9008 | 0:d96713a4739f | 297 | a_motor_3 = 0.999; |
SED9008 | 0:d96713a4739f | 298 | } |
SED9008 | 0:d96713a4739f | 299 | else{m3_set = (a_motor_3+117.5)/100000;} |
SED9008 | 0:d96713a4739f | 300 | |
SED9008 | 0:d96713a4739f | 301 | a_motor_4 = thrust_m4 + ((((float)x/2)-((float)y/2))/CORRECTION_MAGNITUDE); |
SED9008 | 0:d96713a4739f | 302 | thrust_m4 += ((((float)x/2)-((float)y/2))/CORRECTION_MAGNITUDE)/20; |
SED9008 | 0:d96713a4739f | 303 | if(a_motor_4 < 0.1){ |
SED9008 | 0:d96713a4739f | 304 | m4_set = 0.001175; |
SED9008 | 0:d96713a4739f | 305 | a_motor_4 = 0.999; |
SED9008 | 0:d96713a4739f | 306 | } |
SED9008 | 0:d96713a4739f | 307 | else{m4_set = (a_motor_4+117.5)/100000;} |
SED9008 | 0:d96713a4739f | 308 | |
SED9008 | 0:d96713a4739f | 309 | m1.pulsewidth(m1_set); |
SED9008 | 0:d96713a4739f | 310 | m2.pulsewidth(m2_set); |
SED9008 | 0:d96713a4739f | 311 | m3.pulsewidth(m3_set); |
SED9008 | 0:d96713a4739f | 312 | m4.pulsewidth(m4_set); |
SED9008 | 0:d96713a4739f | 313 | alg_enable = 0; |
SED9008 | 0:d96713a4739f | 314 | } |
SED9008 | 0:d96713a4739f | 315 | |
SED9008 | 0:d96713a4739f | 316 | } |
SED9008 | 0:d96713a4739f | 317 | |
SED9008 | 0:d96713a4739f | 318 | void dataSender(void) { |
SED9008 | 0:d96713a4739f | 319 | char buffer [128]; |
SED9008 | 0:d96713a4739f | 320 | sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3); |
SED9008 | 0:d96713a4739f | 321 | tcp_send(buffer); |
SED9008 | 0:d96713a4739f | 322 | } |
SED9008 | 0:d96713a4739f | 323 | |
SED9008 | 0:d96713a4739f | 324 | int main() { |
SED9008 | 0:d96713a4739f | 325 | EthernetErr ethErr = eth.setup(); |
SED9008 | 0:d96713a4739f | 326 | if(ethErr){ |
SED9008 | 0:d96713a4739f | 327 | printf("Error %d in setup.\r\n", ethErr); |
SED9008 | 0:d96713a4739f | 328 | return -1; |
SED9008 | 0:d96713a4739f | 329 | } |
SED9008 | 0:d96713a4739f | 330 | IpAddr ip = eth.getIp(); |
SED9008 | 0:d96713a4739f | 331 | // Set the callbacks for Listening |
SED9008 | 0:d96713a4739f | 332 | ListeningSock.setOnEvent(&onListeningTCPSocketEvent); |
SED9008 | 0:d96713a4739f | 333 | // bind and listen on TCP |
SED9008 | 0:d96713a4739f | 334 | err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT)); |
SED9008 | 0:d96713a4739f | 335 | printf("Binding..\r\n"); |
SED9008 | 0:d96713a4739f | 336 | if(err){printf("Binding Error\n");} |
SED9008 | 0:d96713a4739f | 337 | err=ListeningSock.listen(); // Starts listening |
SED9008 | 0:d96713a4739f | 338 | if(err){printf("Listening Error\r\n");} |
SED9008 | 0:d96713a4739f | 339 | |
SED9008 | 0:d96713a4739f | 340 | m1.pulsewidth(0.001); |
SED9008 | 0:d96713a4739f | 341 | m2.pulsewidth(0.001); |
SED9008 | 0:d96713a4739f | 342 | m3.pulsewidth(0.001); |
SED9008 | 0:d96713a4739f | 343 | m4.pulsewidth(0.001); |
SED9008 | 0:d96713a4739f | 344 | wait(0.3); |
SED9008 | 0:d96713a4739f | 345 | |
SED9008 | 0:d96713a4739f | 346 | //Initialize inertial sensors. |
SED9008 | 0:d96713a4739f | 347 | initializeAccelerometer(); |
SED9008 | 0:d96713a4739f | 348 | initializeGyroscope(); |
SED9008 | 0:d96713a4739f | 349 | |
SED9008 | 0:d96713a4739f | 350 | |
SED9008 | 0:d96713a4739f | 351 | Timer tmr; |
SED9008 | 0:d96713a4739f | 352 | tmr.start(); |
SED9008 | 0:d96713a4739f | 353 | |
SED9008 | 0:d96713a4739f | 354 | while(true) |
SED9008 | 0:d96713a4739f | 355 | { |
SED9008 | 0:d96713a4739f | 356 | Net::poll(); |
SED9008 | 0:d96713a4739f | 357 | if(tmr.read() > 0.2){ |
SED9008 | 0:d96713a4739f | 358 | led4=!led4; |
SED9008 | 0:d96713a4739f | 359 | tmr.reset(); |
SED9008 | 0:d96713a4739f | 360 | } |
SED9008 | 0:d96713a4739f | 361 | |
SED9008 | 0:d96713a4739f | 362 | if(calibrate && !calibrated){ |
SED9008 | 0:d96713a4739f | 363 | calibrateAccelerometer(); |
SED9008 | 0:d96713a4739f | 364 | calibrateGyroscope(); |
SED9008 | 0:d96713a4739f | 365 | led2 = 1; |
SED9008 | 0:d96713a4739f | 366 | calibrated = 1; |
SED9008 | 0:d96713a4739f | 367 | tcp_send("Calibrated\r\n"); |
SED9008 | 0:d96713a4739f | 368 | } |
SED9008 | 0:d96713a4739f | 369 | |
SED9008 | 0:d96713a4739f | 370 | if(calibrated && start && !started){ |
SED9008 | 0:d96713a4739f | 371 | //Accelerometer data rate is 500Hz, so we'll sample at this speed. |
SED9008 | 0:d96713a4739f | 372 | accelerometerTicker.attach(&sampleAccelerometer, 0.002); |
SED9008 | 0:d96713a4739f | 373 | //Gyroscope data rate is 500Hz, so we'll sample at this speed. |
SED9008 | 0:d96713a4739f | 374 | gyroscopeTicker.attach(&sampleGyroscope, 0.002); |
SED9008 | 0:d96713a4739f | 375 | //Update the filter variables at the correct rate. |
SED9008 | 0:d96713a4739f | 376 | filterTicker.attach(&filter, FILTER_RATE); |
SED9008 | 0:d96713a4739f | 377 | dataTicker.attach(&dataSender, 0.2); |
SED9008 | 0:d96713a4739f | 378 | algorithmTicker.attach(&algorithm, 0.001); |
SED9008 | 0:d96713a4739f | 379 | started = 1; |
SED9008 | 0:d96713a4739f | 380 | } |
SED9008 | 0:d96713a4739f | 381 | } |
SED9008 | 0:d96713a4739f | 382 | } |
SED9008 | 0:d96713a4739f | 383 | |
SED9008 | 0:d96713a4739f | 384 | void calibrateAccelerometer(void) { |
SED9008 | 0:d96713a4739f | 385 | |
SED9008 | 0:d96713a4739f | 386 | a_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 387 | a_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 388 | a_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 389 | |
SED9008 | 0:d96713a4739f | 390 | //Take a number of readings and average them |
SED9008 | 0:d96713a4739f | 391 | //to calculate the zero g offset. |
SED9008 | 0:d96713a4739f | 392 | for (int i = 0; i < CALIBRATION_SAMPLES; i++) { |
SED9008 | 0:d96713a4739f | 393 | |
SED9008 | 0:d96713a4739f | 394 | accelerometer.getOutput(readings); |
SED9008 | 0:d96713a4739f | 395 | |
SED9008 | 0:d96713a4739f | 396 | a_xAccumulator += (int16_t) readings[0]; |
SED9008 | 0:d96713a4739f | 397 | a_yAccumulator += (int16_t) readings[1]; |
SED9008 | 0:d96713a4739f | 398 | a_zAccumulator += (int16_t) readings[2]; |
SED9008 | 0:d96713a4739f | 399 | |
SED9008 | 0:d96713a4739f | 400 | wait(ACC_RATE); |
SED9008 | 0:d96713a4739f | 401 | |
SED9008 | 0:d96713a4739f | 402 | } |
SED9008 | 0:d96713a4739f | 403 | |
SED9008 | 0:d96713a4739f | 404 | a_xAccumulator /= CALIBRATION_SAMPLES; |
SED9008 | 0:d96713a4739f | 405 | a_yAccumulator /= CALIBRATION_SAMPLES; |
SED9008 | 0:d96713a4739f | 406 | a_zAccumulator /= CALIBRATION_SAMPLES; |
SED9008 | 0:d96713a4739f | 407 | |
SED9008 | 0:d96713a4739f | 408 | //At 4mg/LSB, 250 LSBs is 1g. |
SED9008 | 0:d96713a4739f | 409 | a_xBias = a_xAccumulator; |
SED9008 | 0:d96713a4739f | 410 | a_yBias = a_yAccumulator; |
SED9008 | 0:d96713a4739f | 411 | a_zBias = (a_zAccumulator - 250); |
SED9008 | 0:d96713a4739f | 412 | |
SED9008 | 0:d96713a4739f | 413 | a_xAccumulator = 0; |
SED9008 | 0:d96713a4739f | 414 | a_yAccumulator = 0; |
SED9008 | 0:d96713a4739f | 415 | a_zAccumulator = 0; |
SED9008 | 0:d96713a4739f | 416 | } |
SED9008 | 0:d96713a4739f | 417 | |
SED9008 | 0:d96713a4739f | 418 | void tcp_send( const char* data ){ |
SED9008 | 0:d96713a4739f | 419 | int len = strlen(data); |
SED9008 | 0:d96713a4739f | 420 | pConnectedSock->send(data, len); |
SED9008 | 0:d96713a4739f | 421 | } |
SED9008 | 0:d96713a4739f | 422 | |
SED9008 | 0:d96713a4739f | 423 | void onConnectedTCPSocketEvent(TCPSocketEvent e) |
SED9008 | 0:d96713a4739f | 424 | { |
SED9008 | 0:d96713a4739f | 425 | switch(e) |
SED9008 | 0:d96713a4739f | 426 | { |
SED9008 | 0:d96713a4739f | 427 | case TCPSOCKET_CONNECTED: |
SED9008 | 0:d96713a4739f | 428 | led1 = 1; |
SED9008 | 0:d96713a4739f | 429 | printf("TCP Socket Connected\r\n"); |
SED9008 | 0:d96713a4739f | 430 | break; |
SED9008 | 0:d96713a4739f | 431 | case TCPSOCKET_WRITEABLE: |
SED9008 | 0:d96713a4739f | 432 | led1 = 1; |
SED9008 | 0:d96713a4739f | 433 | //Can now write some data... |
SED9008 | 0:d96713a4739f | 434 | printf("TCP Socket Writable\r\n"); |
SED9008 | 0:d96713a4739f | 435 | break; |
SED9008 | 0:d96713a4739f | 436 | case TCPSOCKET_READABLE: |
SED9008 | 0:d96713a4739f | 437 | led1 = 1; |
SED9008 | 0:d96713a4739f | 438 | //Can now read dome data... |
SED9008 | 0:d96713a4739f | 439 | printf("TCP Socket Readable\r\n"); |
SED9008 | 0:d96713a4739f | 440 | // Read in any available data into the buffer |
SED9008 | 0:d96713a4739f | 441 | char buff[128]; |
SED9008 | 0:d96713a4739f | 442 | while ( int len = pConnectedSock->recv(buff, 128) ) { |
SED9008 | 0:d96713a4739f | 443 | // And send straight back out again |
SED9008 | 0:d96713a4739f | 444 | //pConnectedSock->send(buff, len); |
SED9008 | 0:d96713a4739f | 445 | int test = buff[0]; |
SED9008 | 0:d96713a4739f | 446 | char buffer[128]; |
SED9008 | 0:d96713a4739f | 447 | sprintf(buffer, "|%i|\r\n", test); |
SED9008 | 0:d96713a4739f | 448 | tcp_send(buffer); |
SED9008 | 0:d96713a4739f | 449 | if(test == 99) {calibrate = 1;} |
SED9008 | 0:d96713a4739f | 450 | if(test == 115) {start = 1;} |
SED9008 | 0:d96713a4739f | 451 | buff[len]=0; // make terminater |
SED9008 | 0:d96713a4739f | 452 | printf("Received&Wrote:%s\r\n",buff); |
SED9008 | 0:d96713a4739f | 453 | } |
SED9008 | 0:d96713a4739f | 454 | break; |
SED9008 | 0:d96713a4739f | 455 | case TCPSOCKET_CONTIMEOUT: |
SED9008 | 0:d96713a4739f | 456 | printf("TCP Socket Timeout\r\n"); |
SED9008 | 0:d96713a4739f | 457 | break; |
SED9008 | 0:d96713a4739f | 458 | case TCPSOCKET_CONRST: |
SED9008 | 0:d96713a4739f | 459 | printf("TCP Socket CONRST\r\n"); |
SED9008 | 0:d96713a4739f | 460 | break; |
SED9008 | 0:d96713a4739f | 461 | case TCPSOCKET_CONABRT: |
SED9008 | 0:d96713a4739f | 462 | printf("TCP Socket CONABRT\r\n"); |
SED9008 | 0:d96713a4739f | 463 | break; |
SED9008 | 0:d96713a4739f | 464 | case TCPSOCKET_ERROR: |
SED9008 | 0:d96713a4739f | 465 | printf("TCP Socket Error\r\n"); |
SED9008 | 0:d96713a4739f | 466 | break; |
SED9008 | 0:d96713a4739f | 467 | case TCPSOCKET_DISCONNECTED: |
SED9008 | 0:d96713a4739f | 468 | //Close socket... |
SED9008 | 0:d96713a4739f | 469 | printf("TCP Socket Disconnected\r\n"); |
SED9008 | 0:d96713a4739f | 470 | pConnectedSock->close(); |
SED9008 | 0:d96713a4739f | 471 | break; |
SED9008 | 0:d96713a4739f | 472 | default: |
SED9008 | 0:d96713a4739f | 473 | printf("DEFAULT\r\n"); |
SED9008 | 0:d96713a4739f | 474 | } |
SED9008 | 0:d96713a4739f | 475 | } |
SED9008 | 0:d96713a4739f | 476 | |
SED9008 | 0:d96713a4739f | 477 | void onListeningTCPSocketEvent(TCPSocketEvent e) |
SED9008 | 0:d96713a4739f | 478 | { |
SED9008 | 0:d96713a4739f | 479 | switch(e) |
SED9008 | 0:d96713a4739f | 480 | { |
SED9008 | 0:d96713a4739f | 481 | case TCPSOCKET_ACCEPT: |
SED9008 | 0:d96713a4739f | 482 | printf("Listening: TCP Socket Accepted\r\n"); |
SED9008 | 0:d96713a4739f | 483 | // Accepts connection from client and gets connected socket. |
SED9008 | 0:d96713a4739f | 484 | err=ListeningSock.accept(&client, &pConnectedSock); |
SED9008 | 0:d96713a4739f | 485 | if (err) { |
SED9008 | 0:d96713a4739f | 486 | printf("onListeningTcpSocketEvent : Could not accept connection.\r\n"); |
SED9008 | 0:d96713a4739f | 487 | return; //Error in accept, discard connection |
SED9008 | 0:d96713a4739f | 488 | } |
SED9008 | 0:d96713a4739f | 489 | // Setup the new socket events |
SED9008 | 0:d96713a4739f | 490 | pConnectedSock->setOnEvent(&onConnectedTCPSocketEvent); |
SED9008 | 0:d96713a4739f | 491 | // We can find out from where the connection is coming by looking at the |
SED9008 | 0:d96713a4739f | 492 | // Host parameter of the accept() method |
SED9008 | 0:d96713a4739f | 493 | IpAddr clientIp = client.getIp(); |
SED9008 | 0:d96713a4739f | 494 | printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n", |
SED9008 | 0:d96713a4739f | 495 | clientIp[0], clientIp[1], clientIp[2], clientIp[3]); |
SED9008 | 0:d96713a4739f | 496 | break; |
SED9008 | 0:d96713a4739f | 497 | // the following cases will not happen |
SED9008 | 0:d96713a4739f | 498 | case TCPSOCKET_CONNECTED: |
SED9008 | 0:d96713a4739f | 499 | printf("Listening: TCP Socket Connected\r\n"); |
SED9008 | 0:d96713a4739f | 500 | break; |
SED9008 | 0:d96713a4739f | 501 | case TCPSOCKET_WRITEABLE: |
SED9008 | 0:d96713a4739f | 502 | printf("Listening: TCP Socket Writable\r\n"); |
SED9008 | 0:d96713a4739f | 503 | break; |
SED9008 | 0:d96713a4739f | 504 | case TCPSOCKET_READABLE: |
SED9008 | 0:d96713a4739f | 505 | printf("Listening: TCP Socket Readable\r\n"); |
SED9008 | 0:d96713a4739f | 506 | break; |
SED9008 | 0:d96713a4739f | 507 | case TCPSOCKET_CONTIMEOUT: |
SED9008 | 0:d96713a4739f | 508 | printf("Listening: TCP Socket Timeout\r\n"); |
SED9008 | 0:d96713a4739f | 509 | break; |
SED9008 | 0:d96713a4739f | 510 | case TCPSOCKET_CONRST: |
SED9008 | 0:d96713a4739f | 511 | printf("Listening: TCP Socket CONRST\r\n"); |
SED9008 | 0:d96713a4739f | 512 | break; |
SED9008 | 0:d96713a4739f | 513 | case TCPSOCKET_CONABRT: |
SED9008 | 0:d96713a4739f | 514 | printf("Listening: TCP Socket CONABRT\r\n"); |
SED9008 | 0:d96713a4739f | 515 | break; |
SED9008 | 0:d96713a4739f | 516 | case TCPSOCKET_ERROR: |
SED9008 | 0:d96713a4739f | 517 | printf("Listening: TCP Socket Error\r\n"); |
SED9008 | 0:d96713a4739f | 518 | break; |
SED9008 | 0:d96713a4739f | 519 | case TCPSOCKET_DISCONNECTED: |
SED9008 | 0:d96713a4739f | 520 | //Close socket... |
SED9008 | 0:d96713a4739f | 521 | printf("Listening: TCP Socket Disconnected\r\n"); |
SED9008 | 0:d96713a4739f | 522 | ListeningSock.close(); |
SED9008 | 0:d96713a4739f | 523 | break; |
SED9008 | 0:d96713a4739f | 524 | default: |
SED9008 | 0:d96713a4739f | 525 | printf("DEFAULT\r\n"); |
SED9008 | 0:d96713a4739f | 526 | }; |
SED9008 | 0:d96713a4739f | 527 | } |
SED9008 | 0:d96713a4739f | 528 | |
SED9008 | 0:d96713a4739f | 529 | void getI2CAddress() |
SED9008 | 0:d96713a4739f | 530 | { |
SED9008 | 0:d96713a4739f | 531 | int count = 1; |
SED9008 | 0:d96713a4739f | 532 | for (int address=0; address<256; address+=2) { |
SED9008 | 0:d96713a4739f | 533 | if (!i2c.write(address, NULL, 0)) { // 0 returned is ok |
SED9008 | 0:d96713a4739f | 534 | char buffer [128]; |
SED9008 | 0:d96713a4739f | 535 | sprintf (buffer, "%i: - %i\n",count, address); |
SED9008 | 0:d96713a4739f | 536 | tcp_send(buffer); |
SED9008 | 0:d96713a4739f | 537 | count++; |
SED9008 | 0:d96713a4739f | 538 | } |
SED9008 | 0:d96713a4739f | 539 | } |
SED9008 | 0:d96713a4739f | 540 | } |