Leo Veenstra / Mbed 2 deprecated TCP_control

Dependencies:   EthernetNetIf mbed

main.cpp@3:849ebcaa62c7, 2012-04-24 (annotated)

Committer:
SED9008
Date:
Tue Apr 24 14:12:54 2012 +0000
Revision:
3:849ebcaa62c7
Parent:
2:53614df77a8e 
sofar

Who changed what in which revision?

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