Drone - Last update drone code.

Users » SED9008 » Code » Drone

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	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 May 2012
Imports:	43
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	6
Followers:	3

main.cpp@0:d96713a4739f, 2012-06-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning