TCP_server - Gyro output to TCP in degrees with calibration an…

Users » SED9008 » Code » TCP_server

Leo Veenstra / Mbed 2 deprecated TCP_server

Gyro output to TCP in degrees with calibration and correction

main.cpp@2:96f81996a332, 2012-03-19 (annotated)

Committer:: SED9008
Date:: Mon Mar 19 11:06:20 2012 +0000
Revision:: 2:96f81996a332
Parent:: 1:a88d1309f810

Who changed what in which revision?

User	Revision	Line number	New contents of line
SED9008	0:5de940cf9783	1
SED9008	0:5de940cf9783	2	// TCP Echo server
SED9008	0:5de940cf9783	3	// 2010/9/7
SED9008	0:5de940cf9783	4
SED9008	0:5de940cf9783	5
SED9008	0:5de940cf9783	6	/*
SED9008	0:5de940cf9783	7
SED9008	0:5de940cf9783	8	(Execute Sample)
SED9008	0:5de940cf9783	9	PC side:
SED9008	0:5de940cf9783	10	telnet 192.168.0.25 12345
SED9008	0:5de940cf9783	11	Trying 192.168.0.25...
SED9008	0:5de940cf9783	12	Connected to 192.168.0.25.
SED9008	0:5de940cf9783	13	Escape character is '^]'.
SED9008	0:5de940cf9783	14
SED9008	0:5de940cf9783	15	mbed side:
SED9008	0:5de940cf9783	16	Setup OK
SED9008	0:5de940cf9783	17	mbed IP Address is 192.168.0.25
SED9008	0:5de940cf9783	18	Binding..
SED9008	0:5de940cf9783	19	Listening...
SED9008	0:5de940cf9783	20	Listening: TCP Socket Accepted
SED9008	0:5de940cf9783	21	Listening: Incoming TCP connection from 192.168.0.7
SED9008	0:5de940cf9783	22	TCP Socket Readable
SED9008	0:5de940cf9783	23	Received&Wrote:test text
SED9008	0:5de940cf9783	24
SED9008	0:5de940cf9783	25	TCP Socket Writable
SED9008	0:5de940cf9783	26
SED9008	0:5de940cf9783	27
SED9008	0:5de940cf9783	28	*/
SED9008	0:5de940cf9783	29
SED9008	1:a88d1309f810	30
SED9008	1:a88d1309f810	31	//############## INCLUDE FILES ##############//
SED9008	0:5de940cf9783	32	#include "mbed.h"
SED9008	0:5de940cf9783	33	#include "EthernetNetIf.h"
SED9008	0:5de940cf9783	34	#include "TCPSocket.h"
SED9008	0:5de940cf9783	35	#include "ADXL345_I2C.h"
SED9008	2:96f81996a332	36	#include "L3G4200D.h"
SED9008	1:a88d1309f810	37	//#############################################//
SED9008	0:5de940cf9783	38
SED9008	1:a88d1309f810	39	//############## DEFINES ##############//
SED9008	0:5de940cf9783	40	#define TCP_LISTENING_PORT 1337
SED9008	1:a88d1309f810	41	#define PI 3.14159265
SED9008	1:a88d1309f810	42	//#############################################//
SED9008	0:5de940cf9783	43
SED9008	1:a88d1309f810	44	//############## VARIABLES ##############//
SED9008	1:a88d1309f810	45	double x_cor = 0,
SED9008	1:a88d1309f810	46	y_cor = 0,
SED9008	1:a88d1309f810	47	cont[2] = {0,0};
SED9008	1:a88d1309f810	48	int print = 0,
SED9008	1:a88d1309f810	49	go = 0,
SED9008	1:a88d1309f810	50	calibrated = 0;
SED9008	2:96f81996a332	51	int gyro_val[3] = {0,0,0};
SED9008	1:a88d1309f810	52	//#############################################//
SED9008	1:a88d1309f810	53
SED9008	1:a88d1309f810	54	//##############PIN CONFIGURATION##############//
SED9008	0:5de940cf9783	55	DigitalOut led3(LED3, "led3");
SED9008	0:5de940cf9783	56	DigitalOut led4(LED4, "led4");
SED9008	0:5de940cf9783	57	DigitalIn init(p5);
SED9008	0:5de940cf9783	58	DigitalIn calibrate(p20);
SED9008	2:96f81996a332	59	I2C i2c(p9, p10); // sda, scl
SED9008	0:5de940cf9783	60	ADXL345_I2C acc(p9, p10);
SED9008	2:96f81996a332	61	L3G4200D gyro(p9, p10);
SED9008	1:a88d1309f810	62	//#############################################//
SED9008	0:5de940cf9783	63
SED9008	1:a88d1309f810	64	//############## PROTOTYPES ##############//
SED9008	0:5de940cf9783	65	void tcp_send(const char*);
SED9008	0:5de940cf9783	66	void tcp_sendIfConnected( const char* data );
SED9008	0:5de940cf9783	67	void onConnectedTCPSocketEvent(TCPSocketEvent e);
SED9008	0:5de940cf9783	68	void onListeningTCPSocketEvent(TCPSocketEvent e);
SED9008	1:a88d1309f810	69	double getAngle(char axis, int precision);
SED9008	2:96f81996a332	70	void getI2CAddress();
SED9008	1:a88d1309f810	71	//#############################################//
SED9008	0:5de940cf9783	72
SED9008	1:a88d1309f810	73	//############## ETHERNET CONFIG ##############//
SED9008	0:5de940cf9783	74	// EthernetNetIf eth;
SED9008	0:5de940cf9783	75	EthernetNetIf eth(
SED9008	0:5de940cf9783	76	IpAddr(192,168,0,16), //IP Address
SED9008	0:5de940cf9783	77	IpAddr(255,255,255,0), //Network Mask
SED9008	2:96f81996a332	78	IpAddr(192,168,0,2), //Gateway
SED9008	2:96f81996a332	79	IpAddr(192,168,0,2) //DNS
SED9008	0:5de940cf9783	80	);
SED9008	0:5de940cf9783	81
SED9008	0:5de940cf9783	82	TCPSocket ListeningSock;
SED9008	0:5de940cf9783	83	TCPSocket* pConnectedSock; // for ConnectedSock
SED9008	0:5de940cf9783	84	Host client;
SED9008	0:5de940cf9783	85	TCPSocketErr err;
SED9008	1:a88d1309f810	86	//#############################################//
SED9008	1:a88d1309f810	87
SED9008	0:5de940cf9783	88	Ticker routine;
SED9008	0:5de940cf9783	89
SED9008	1:a88d1309f810	90	//############## INTERRUPT ##############//
SED9008	0:5de940cf9783	91	void interrupt()
SED9008	0:5de940cf9783	92	{
SED9008	1:a88d1309f810	93	if(go){
SED9008	1:a88d1309f810	94	if(calibrated){
SED9008	1:a88d1309f810	95	if(print){
SED9008	2:96f81996a332	96	// char buffer [128];
SED9008	2:96f81996a332	97	// sprintf (buffer, "x:%f - y:%f\n",cont[0],cont[1]);
SED9008	2:96f81996a332	98	// tcp_send(buffer);
SED9008	1:a88d1309f810	99	char buffer [128];
SED9008	2:96f81996a332	100	sprintf (buffer, "x:%i - y:%i - z:%i\n",gyro_val[0],gyro_val[1],gyro_val[2]);
SED9008	2:96f81996a332	101	tcp_send(buffer);
SED9008	1:a88d1309f810	102	print = 0;
SED9008	1:a88d1309f810	103	}
SED9008	1:a88d1309f810	104	}
SED9008	1:a88d1309f810	105	}
SED9008	1:a88d1309f810	106	}
SED9008	1:a88d1309f810	107	//#############################################//
SED9008	0:5de940cf9783	108
SED9008	1:a88d1309f810	109	//############## MAIN ##############//
SED9008	0:5de940cf9783	110	int main() {
SED9008	1:a88d1309f810	111	//##############ETHERNET&TCP INIT##############//
SED9008	0:5de940cf9783	112	EthernetErr ethErr = eth.setup();
SED9008	0:5de940cf9783	113	if(ethErr)
SED9008	0:5de940cf9783	114	{
SED9008	0:5de940cf9783	115	return -1;
SED9008	1:a88d1309f810	116	}
SED9008	1:a88d1309f810	117	IpAddr ip = eth.getIp();
SED9008	0:5de940cf9783	118	// Set the callbacks for Listening
SED9008	1:a88d1309f810	119	ListeningSock.setOnEvent(&onListeningTCPSocketEvent);
SED9008	0:5de940cf9783	120	// bind and listen on TCP
SED9008	0:5de940cf9783	121	err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT));
SED9008	0:5de940cf9783	122	if(err)
SED9008	0:5de940cf9783	123	{
SED9008	0:5de940cf9783	124	//Deal with that error...
SED9008	1:a88d1309f810	125	}
SED9008	0:5de940cf9783	126	err=ListeningSock.listen(); // Starts listening
SED9008	0:5de940cf9783	127	if(err)
SED9008	0:5de940cf9783	128	{
SED9008	0:5de940cf9783	129	//listening error
SED9008	0:5de940cf9783	130	}
SED9008	1:a88d1309f810	131	//#############################################//
SED9008	0:5de940cf9783	132
SED9008	1:a88d1309f810	133	//#############ACCELEROMETER INIT##############//
SED9008	0:5de940cf9783	134	//Go into standby mode to configure the device.
SED9008	0:5de940cf9783	135	acc.setPowerControl(0x00);
SED9008	0:5de940cf9783	136	//Full resolution, +/-16g, 4mg/LSB.
SED9008	1:a88d1309f810	137	acc.setDataFormatControl(0x00);
SED9008	0:5de940cf9783	138	//3.2kHz data rate.
SED9008	0:5de940cf9783	139	acc.setDataRate(ADXL345_3200HZ);
SED9008	0:5de940cf9783	140	//Measurement mode.
SED9008	0:5de940cf9783	141	acc.setPowerControl(0x08);
SED9008	0:5de940cf9783	142	wait(0.1);
SED9008	1:a88d1309f810	143	//#############################################//
SED9008	1:a88d1309f810	144
SED9008	2:96f81996a332	145	//##################GYRO INIT##################//
SED9008	2:96f81996a332	146
SED9008	2:96f81996a332	147
SED9008	2:96f81996a332	148	//#############################################//
SED9008	2:96f81996a332	149
SED9008	0:5de940cf9783	150	Timer tmr;
SED9008	0:5de940cf9783	151	tmr.start();
SED9008	1:a88d1309f810	152
SED9008	0:5de940cf9783	153	while(true)
SED9008	0:5de940cf9783	154	{
SED9008	0:5de940cf9783	155	Net::poll();
SED9008	0:5de940cf9783	156	while(init)
SED9008	0:5de940cf9783	157	{
SED9008	0:5de940cf9783	158	if(!init)
SED9008	0:5de940cf9783	159	{
SED9008	0:5de940cf9783	160	tcp_send("You are a go!\n");
SED9008	0:5de940cf9783	161	led3 = 1;
SED9008	0:5de940cf9783	162	go = 1;
SED9008	0:5de940cf9783	163	}
SED9008	0:5de940cf9783	164	}
SED9008	0:5de940cf9783	165	if(tmr.read() > 0.2) // sec
SED9008	0:5de940cf9783	166	{
SED9008	0:5de940cf9783	167	led4=!led4; //Show that we are alive
SED9008	0:5de940cf9783	168	tmr.reset();
SED9008	0:5de940cf9783	169	}
SED9008	2:96f81996a332	170	/* if(calibrate){
SED9008	0:5de940cf9783	171	while(calibrate){};
SED9008	2:96f81996a332	172	char buffer [128];
SED9008	2:96f81996a332	173	sprintf (buffer, "Calibrated\n");
SED9008	2:96f81996a332	174	tcp_send(buffer);
SED9008	2:96f81996a332	175
SED9008	2:96f81996a332	176
SED9008	2:96f81996a332	177	x_cor = getAngle('x',100);//-90
SED9008	2:96f81996a332	178	y_cor = getAngle('y',100);//-90
SED9008	1:a88d1309f810	179
SED9008	1:a88d1309f810	180	char buffer [128];
SED9008	1:a88d1309f810	181	sprintf (buffer, "Calibration values:\n");
SED9008	1:a88d1309f810	182	tcp_send(buffer);
SED9008	1:a88d1309f810	183	sprintf (buffer, "x:%f - y:%f\n",x_cor, y_cor);
SED9008	1:a88d1309f810	184	tcp_send(buffer);
SED9008	1:a88d1309f810	185	sprintf (buffer, "------------------------\n");
SED9008	1:a88d1309f810	186	tcp_send(buffer);
SED9008	1:a88d1309f810	187
SED9008	1:a88d1309f810	188	calibrated = 1;
SED9008	1:a88d1309f810	189
SED9008	1:a88d1309f810	190	wait(0.1);
SED9008	1:a88d1309f810	191
SED9008	1:a88d1309f810	192	routine.attach_us(&interrupt,10);
SED9008	1:a88d1309f810	193	}
SED9008	1:a88d1309f810	194	if(calibrated){
SED9008	1:a88d1309f810	195
SED9008	2:96f81996a332	196	// cont[0] = getAngle('x',10) - x_cor;
SED9008	2:96f81996a332	197	// cont[1] = getAngle('y',10) - y_cor;
SED9008	2:96f81996a332	198	gyro.read(gyro_val);
SED9008	1:a88d1309f810	199	print = 1;
SED9008	2:96f81996a332	200	}*/
SED9008	2:96f81996a332	201	if(go){
SED9008	2:96f81996a332	202	gyro.read(gyro_val);
SED9008	2:96f81996a332	203	char buffer [128];
SED9008	2:96f81996a332	204	sprintf (buffer, "x:%i - y:%i - z:%i\n",gyro_val[0],gyro_val[1],gyro_val[2]);
SED9008	2:96f81996a332	205	tcp_send(buffer);
SED9008	0:5de940cf9783	206	}
SED9008	2:96f81996a332	207
SED9008	0:5de940cf9783	208	}
SED9008	0:5de940cf9783	209	}
SED9008	1:a88d1309f810	210	//#############################################//
SED9008	1:a88d1309f810	211
SED9008	1:a88d1309f810	212	//############## SUBROUTINES ##############//
SED9008	0:5de940cf9783	213
SED9008	0:5de940cf9783	214	void tcp_send( const char* data ){
SED9008	0:5de940cf9783	215	int len = strlen(data);
SED9008	0:5de940cf9783	216	pConnectedSock->send(data, len);
SED9008	0:5de940cf9783	217	}
SED9008	0:5de940cf9783	218
SED9008	0:5de940cf9783	219	void onConnectedTCPSocketEvent(TCPSocketEvent e)
SED9008	0:5de940cf9783	220	{
SED9008	0:5de940cf9783	221	switch(e)
SED9008	0:5de940cf9783	222	{
SED9008	0:5de940cf9783	223	case TCPSOCKET_CONNECTED:
SED9008	0:5de940cf9783	224	break;
SED9008	0:5de940cf9783	225	case TCPSOCKET_WRITEABLE:
SED9008	0:5de940cf9783	226	//Can now write some data...
SED9008	0:5de940cf9783	227	printf("TCP Socket Writable\r\n");
SED9008	0:5de940cf9783	228	break;
SED9008	0:5de940cf9783	229	case TCPSOCKET_READABLE:
SED9008	0:5de940cf9783	230	//Can now read dome data...
SED9008	0:5de940cf9783	231	// Read in any available data into the buffer
SED9008	0:5de940cf9783	232	//char buff[128];
SED9008	0:5de940cf9783	233	//while ( int len = pConnectedSock->recv(buff, 128) ) {
SED9008	0:5de940cf9783	234	// And send straight back out again
SED9008	0:5de940cf9783	235	// pConnectedSock->send(buff, len);
SED9008	0:5de940cf9783	236	// buff[len]=0; // make terminater
SED9008	0:5de940cf9783	237	//}
SED9008	0:5de940cf9783	238	break;
SED9008	0:5de940cf9783	239	case TCPSOCKET_CONTIMEOUT:
SED9008	0:5de940cf9783	240	printf("TCP Socket Timeout\r\n");
SED9008	0:5de940cf9783	241	break;
SED9008	0:5de940cf9783	242	case TCPSOCKET_CONRST:
SED9008	0:5de940cf9783	243	printf("TCP Socket CONRST\r\n");
SED9008	0:5de940cf9783	244	break;
SED9008	0:5de940cf9783	245	case TCPSOCKET_CONABRT:
SED9008	0:5de940cf9783	246	printf("TCP Socket CONABRT\r\n");
SED9008	0:5de940cf9783	247	break;
SED9008	0:5de940cf9783	248	case TCPSOCKET_ERROR:
SED9008	0:5de940cf9783	249	printf("TCP Socket Error\r\n");
SED9008	0:5de940cf9783	250	break;
SED9008	0:5de940cf9783	251	case TCPSOCKET_DISCONNECTED:
SED9008	0:5de940cf9783	252	//Close socket...
SED9008	0:5de940cf9783	253	printf("TCP Socket Disconnected\r\n");
SED9008	0:5de940cf9783	254	pConnectedSock->close();
SED9008	0:5de940cf9783	255	break;
SED9008	0:5de940cf9783	256	default:
SED9008	0:5de940cf9783	257	printf("DEFAULT\r\n");
SED9008	0:5de940cf9783	258	}
SED9008	0:5de940cf9783	259	}
SED9008	0:5de940cf9783	260
SED9008	0:5de940cf9783	261	void onListeningTCPSocketEvent(TCPSocketEvent e)
SED9008	0:5de940cf9783	262	{
SED9008	0:5de940cf9783	263	switch(e)
SED9008	0:5de940cf9783	264	{
SED9008	0:5de940cf9783	265	case TCPSOCKET_ACCEPT:
SED9008	0:5de940cf9783	266	printf("Listening: TCP Socket Accepted\r\n");
SED9008	0:5de940cf9783	267	// Accepts connection from client and gets connected socket.
SED9008	0:5de940cf9783	268	err=ListeningSock.accept(&client, &pConnectedSock);
SED9008	0:5de940cf9783	269	if (err) {
SED9008	0:5de940cf9783	270	printf("onListeningTcpSocketEvent : Could not accept connection.\r\n");
SED9008	0:5de940cf9783	271	return; //Error in accept, discard connection
SED9008	0:5de940cf9783	272	}
SED9008	0:5de940cf9783	273	// Setup the new socket events
SED9008	0:5de940cf9783	274	pConnectedSock->setOnEvent(&onConnectedTCPSocketEvent);
SED9008	0:5de940cf9783	275	// We can find out from where the connection is coming by looking at the
SED9008	0:5de940cf9783	276	// Host parameter of the accept() method
SED9008	0:5de940cf9783	277	IpAddr clientIp = client.getIp();
SED9008	0:5de940cf9783	278	printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n",
SED9008	0:5de940cf9783	279	clientIp[0], clientIp[1], clientIp[2], clientIp[3]);
SED9008	0:5de940cf9783	280	break;
SED9008	0:5de940cf9783	281	// the following cases will not happen
SED9008	0:5de940cf9783	282	case TCPSOCKET_CONNECTED:
SED9008	0:5de940cf9783	283	printf("Listening: TCP Socket Connected\r\n");
SED9008	0:5de940cf9783	284	break;
SED9008	0:5de940cf9783	285	case TCPSOCKET_WRITEABLE:
SED9008	0:5de940cf9783	286	printf("Listening: TCP Socket Writable\r\n");
SED9008	0:5de940cf9783	287	break;
SED9008	0:5de940cf9783	288	case TCPSOCKET_READABLE:
SED9008	0:5de940cf9783	289	printf("Listening: TCP Socket Readable\r\n");
SED9008	0:5de940cf9783	290	break;
SED9008	0:5de940cf9783	291	case TCPSOCKET_CONTIMEOUT:
SED9008	0:5de940cf9783	292	printf("Listening: TCP Socket Timeout\r\n");
SED9008	0:5de940cf9783	293	break;
SED9008	0:5de940cf9783	294	case TCPSOCKET_CONRST:
SED9008	0:5de940cf9783	295	printf("Listening: TCP Socket CONRST\r\n");
SED9008	0:5de940cf9783	296	break;
SED9008	0:5de940cf9783	297	case TCPSOCKET_CONABRT:
SED9008	0:5de940cf9783	298	printf("Listening: TCP Socket CONABRT\r\n");
SED9008	0:5de940cf9783	299	break;
SED9008	0:5de940cf9783	300	case TCPSOCKET_ERROR:
SED9008	0:5de940cf9783	301	printf("Listening: TCP Socket Error\r\n");
SED9008	0:5de940cf9783	302	break;
SED9008	0:5de940cf9783	303	case TCPSOCKET_DISCONNECTED:
SED9008	0:5de940cf9783	304	//Close socket...
SED9008	0:5de940cf9783	305	printf("Listening: TCP Socket Disconnected\r\n");
SED9008	0:5de940cf9783	306	ListeningSock.close();
SED9008	0:5de940cf9783	307	break;
SED9008	0:5de940cf9783	308	default:
SED9008	0:5de940cf9783	309	printf("DEFAULT\r\n");
SED9008	0:5de940cf9783	310	};
SED9008	0:5de940cf9783	311	}
SED9008	1:a88d1309f810	312
SED9008	1:a88d1309f810	313	double getAngle(char axis, int precision)
SED9008	1:a88d1309f810	314	{
SED9008	1:a88d1309f810	315	int measurement[3] = {0,0,0};
SED9008	1:a88d1309f810	316	double x_temp = 0,
SED9008	1:a88d1309f810	317	y_temp = 0,
SED9008	1:a88d1309f810	318	z_temp = 0,
SED9008	1:a88d1309f810	319	x_angle = 0,
SED9008	1:a88d1309f810	320	y_angle = 0;
SED9008	1:a88d1309f810	321
SED9008	1:a88d1309f810	322	int i;
SED9008	1:a88d1309f810	323
SED9008	1:a88d1309f810	324	for(i=0;i<precision;i++)
SED9008	1:a88d1309f810	325	{
SED9008	1:a88d1309f810	326	acc.getOutput(measurement);
SED9008	1:a88d1309f810	327	x_temp = (int16_t)measurement[0];
SED9008	1:a88d1309f810	328	y_temp = (int16_t)measurement[1];
SED9008	1:a88d1309f810	329	z_temp = (int16_t)measurement[2];
SED9008	1:a88d1309f810	330	x_angle += x_temp/z_temp;
SED9008	1:a88d1309f810	331	y_angle += y_temp/z_temp;
SED9008	1:a88d1309f810	332	}
SED9008	1:a88d1309f810	333	x_angle = x_angle/precision;
SED9008	1:a88d1309f810	334	y_angle = y_angle/precision;
SED9008	1:a88d1309f810	335	// x_angle = atan(x_angle) * 180 / PI;
SED9008	1:a88d1309f810	336	// y_angle = atan(y_angle) * 180 / PI;
SED9008	2:96f81996a332	337	if(axis == 'x')return (atan(x_angle) * 180 / PI);//+90
SED9008	2:96f81996a332	338	if(axis == 'y')return (atan(y_angle) * 180 / PI);//+90
SED9008	1:a88d1309f810	339	}
SED9008	1:a88d1309f810	340
SED9008	2:96f81996a332	341	void getI2CAddress()
SED9008	2:96f81996a332	342	{
SED9008	2:96f81996a332	343	int count = 1;
SED9008	2:96f81996a332	344	for (int address=0; address<256; address+=2) {
SED9008	2:96f81996a332	345	if (!i2c.write(address, NULL, 0)) { // 0 returned is ok
SED9008	2:96f81996a332	346	char buffer [128];
SED9008	2:96f81996a332	347	sprintf (buffer, "%i: - %i\n",count, address);
SED9008	2:96f81996a332	348	tcp_send(buffer);
SED9008	2:96f81996a332	349	count++;
SED9008	2:96f81996a332	350	}
SED9008	2:96f81996a332	351	}
SED9008	2:96f81996a332	352	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	01 Mar 2012
Imports:	11
Forks:	0
Commits:	3
Dependents:	0
Dependencies:	4
Followers:	1

main.cpp@2:96f81996a332, 2012-03-19 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning