Example #1
0
int test_frame_ctl(uint8_t key[SHAREDKEY_BYTESIZE]) {
#define	TESTBUFSIZE	4096
	int i,ret;
	ssize_t crypted_len;
	struct frame_st *crypted_frame = NULL;
	internal_msg_t *ori_frame_body;
	internal_msg_t decrypt_frame_body;

	ori_frame_body = malloc(sizeof(*ori_frame_body));
	ori_frame_body->msg_type = MSG_CTL_TYPE;
	ori_frame_body->ctl_frame_body.code = CTL_PIPELINE_OPEN;
	ori_frame_body->ctl_frame_body.arg.pipeline_open.flow_id = 1234;
	ori_frame_body->ctl_frame_body.arg.pipeline_open.max_delay_in_ms = 500;
	ori_frame_body->ctl_frame_body.arg.pipeline_open.reply_frame_flags = 1;
	ori_frame_body->ctl_frame_body.arg.pipeline_open.data = malloc(TESTBUFSIZE);
	ori_frame_body->ctl_frame_body.arg.pipeline_open.data_len = TESTBUFSIZE;
	for (i=0;i<TESTBUFSIZE;++i) {
		ori_frame_body->ctl_frame_body.arg.pipeline_open.data[i] = i%63;
	}

	printf("Original: %d bytes.\n", TESTBUFSIZE);

	crypted_len = frame_encode(&crypted_frame, ori_frame_body, FRAME_FLAG_MAKE(1, 1, 1), key);
	printf("frame_encode() = %d bytes.\n", (int)crypted_len);

	ret = frame_decode(&decrypt_frame_body, crypted_frame, key);
	printf("frame_decode() = %d bytes.\n", decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data_len);

	if (memcmp(ori_frame_body->ctl_frame_body.arg.pipeline_open.data, decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data, TESTBUFSIZE)==0) {
		puts("encrypt_frame_body/decrypt_frame_body OK.");
	} else {
		puts("encrypt_frame_body/decrypt_frame_body FAILed.");
	}
	printf("Original: ");
	bin_dump(stdout, ori_frame_body->ctl_frame_body.arg.pipeline_open.data, TESTBUFSIZE);
	printf("Crypted: ");
	bin_dump(stdout, crypted_frame->frame_body, crypted_len);
	printf("Derypted: ");
	bin_dump(stdout, decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data, decrypt_frame_body.ctl_frame_body.arg.pipeline_open.data_len);

	free(ori_frame_body->ctl_frame_body.arg.pipeline_open.data);
	free(ori_frame_body);
	free(crypted_frame);
	return 0;
}
Example #2
0
int test_frame_data(uint8_t key[SHAREDKEY_BYTESIZE]) {
#define	TESTBUFSIZE	4096
	int i,ret;
	ssize_t crypted_len;
	struct frame_st *crypted_frame = NULL;
	internal_msg_t *ori_frame_body;
	internal_msg_t decrypt_frame_body;

	ori_frame_body = malloc(sizeof(*ori_frame_body));
	ori_frame_body->msg_type = 0;
	ori_frame_body->data_frame_body.flow_id = 12345;
	ori_frame_body->data_frame_body.data = malloc(TESTBUFSIZE);
	ori_frame_body->data_frame_body.data_len = TESTBUFSIZE;
	for (i=0;i<TESTBUFSIZE;++i) {
		ori_frame_body->data_frame_body.data[i] = i%63;
	}

	printf("Original: %d bytes.\n", TESTBUFSIZE);

	crypted_len = frame_encode(&crypted_frame, ori_frame_body, FRAME_FLAG_MAKE(1, 1, 1), key);
	printf("frame_encode() = %d bytes.\n", (int)crypted_len);

	ret = frame_decode(&decrypt_frame_body, crypted_frame, key);
	printf("frame_decode() = %d bytes.\n", decrypt_frame_body.data_frame_body.data_len);

	if (memcmp(ori_frame_body->data_frame_body.data, decrypt_frame_body.data_frame_body.data, TESTBUFSIZE)==0) {
		puts("encrypt_frame_body/decrypt_frame_body OK.");
	} else {
		puts("encrypt_frame_body/decrypt_frame_body FAILed.");
	}
	printf("Original: ");
	bin_dump(stdout, ori_frame_body->data_frame_body.data, TESTBUFSIZE);
	printf("Crypted: ");
	bin_dump(stdout, crypted_frame->frame_body, crypted_len);
	printf("Derypted: ");
	bin_dump(stdout, decrypt_frame_body.data_frame_body.data, decrypt_frame_body.data_frame_body.data_len);

	free(ori_frame_body->data_frame_body.data);
	free(ori_frame_body);
	free(crypted_frame);
	return 0;
}
int main() {
	light_start(0);
	shut_down_in(119.5);
	arm_up();
	claw_open();
	bin_down();
	enable_servos();
	
	collect_poms();
	printf("Collected Initial Poms in base\n");
	
	/*
	forward(70);
	msleep(500);
	left(35, 0);
	msleep(300);
	//face 1st pile
	forward(45);
	pomPileOne();
	*/
	forward(50);
	msleep(500);
	left(28, 0);
	//face 1st pile
	forward(59);	//needs to be exact or else claw will hit table divider
	msleep(500);
	pomPileOne();
	
	/*
	//4/9/16 Test Code
	forward(20);
	msleep(500);
	left(15, 0);
	msleep(300);
	//face 1st pile
	forward(80);
	pomPileOne();
	*/
	
	//go to pile 2
	multforward(45, 0.50);
	msleep(100);
	forward(15);
	msleep(300);
	left(85, ks/2);
	msleep(300);
	backward(2);
	//forward(2);
	pomPileTwo();
	
	
	/*
	if(green == 1 && red == 1) {
		printf("Collected all\n");
		//go to bin
		right(60, ks/2);
		forward(100);
		right(100, ks/2);
		msleep(500);
		bin_mid();
		msleep(500);
		bin_dump();
	}
	else {
		//do pom pile three only if robot does not have both one red and one green
		pomPileThree();
		//go to bin
		left(60, ks/2);
		forward(60);
	}
	*/
	backward(5);
	right(80, ks/2);
	msleep(300);
	forward(130);
	msleep(300);
	right(50, ks/2);	//turn against the wall
	msleep(300);
	backward(5);
	msleep(500);
	/*
	arm_mid();
	msleep(300);
	bin_mid();
	msleep(500);
	bin_dump();
	msleep(1000);
	*/
	right(40, ks/2);
	msleep(500);
	backward(15);
	
	servo(MAIN_ARM, 1200);	//move away arm from box
	msleep(300);
	bin_mid();
	msleep(500);
	bin_dump();
	msleep(1000);
	bin_mid();
	msleep(500);
	bin_dump();
	msleep(1000);
	
	/*
	int i = 0;
	for(i = 0; i < 3; i++) {
		forward(2);
		msleep(300);
		backward(2);
		msleep(300);
	}
	*/
	
	msleep(1000);
	right(30, ks/2);
	forward(90);
	
	msleep(2000);
	disable_servos();
	ao();
	return 0;
}
int main() {
	//light_start(0);
	shut_down_in(119.5);
	arm_up();
	claw_open();
	bin_down();
	enable_servos();
	
	collect_poms();
	printf("Collected Initial Poms in base\n");
	
	/*
	forward(70);
	msleep(500);
	left(35, 0);
	msleep(300);
	//face 1st pile
	forward(45);
	pomPileOne();
	*/
	forward(47);
	msleep(500);
	left(30, 0);
	//face 1st pile
	forward(60);	//needs to be exact or else claw will hit table divider
	pomPileOne();
	
	/*
	//4/9/16 Test Code
	forward(20);
	msleep(500);
	left(15, 0);
	msleep(300);
	//face 1st pile
	forward(80);
	pomPileOne();
	*/
	
	//go to pile 2
	multforward(45, 0.50);
	msleep(100);
	forward(15);
	msleep(300);
	left(85, ks/2);
	msleep(300);
	forward(10);
	pomPileTwo();
	
	if(green == 1 && red == 1) {
		printf("Collected all\n");
		//go to bin
		right(60, ks/2);
		forward(100);
		right(100, ks/2);
		msleep(500);
		bin_mid();
		msleep(500);
		bin_dump();
	}
	else {
		//do pom pile three only if robot does not have both one red and one green
		pomPileThree();
		//go to bin
		left(60, ks/2);
		forward(60);
	}
	
	msleep(2000);
	disable_servos();
	ao();
	return 0;
}
int WirelessProtocol::sendData(byte* data, uint16_t size, uint64_t Pipe, uint16_t SendTimeout)
{
	ProtocolMessage PM;

	if (size % 30 == 0)
		PM.AllLength = size / 30;
	else
		PM.AllLength = size / 30 + 1;

	radio->stopListening();
	radio->openWritingPipe(Pipe);


	DBGPRINT("Send data size: %hu, package count: %hu", size, PM.AllLength);

	unsigned long lastSuccSend = millis();

	for (uint8_t i = 0; i < PM.AllLength; i++)
	{
		if (millis() - lastSuccSend > SendTimeout)
		{
			DBGPRINT("All send timeout");
			return ERROR_TIMEOUT;
		}

		DBGPRINT("Trying send package: %hu", i);
		PM.CurrentNum = i;
		memcpy(PM.data, &(data[i*30]), min(30, size - 30*i));
		bin_dump((char *)(PM.data), 30);
		radio->write(&PM, sizeof(ProtocolMessage));
		long wait_start = millis();

		bool ackR = false;
		while ( !(ackR = radio->available()) && (millis() - wait_start) < PACKET_TIMEOUT)
		{
			radio->stopListening();
			radio->write(&PM, sizeof(ProtocolMessage));
			radio->startListening();
			delay(30);
		}
			
	
		if (ackR)
		{
			uint8_t message_count;
			radio->read(&message_count,sizeof(message_count));
			DBGPRINT("Reciving ack package %hu", message_count);
			if (message_count != i)
			{
				DBGPRINT("Failed send package: %hu", i);
				i--;
			}
			else
			{
				DBGPRINT("Packet send");
				lastSuccSend = millis();
			}
		}
		else
		{
			DBGPRINT("Timeout sending package: %hu", i);
			i--;
		}
	}



	DBGPRINT("Send complete");



	return ERROR_SUCCESS;
}
int WirelessProtocol::reciveData(byte* data, uint16_t MaxSize, uint16_t* size, uint64_t* Pipe, uint16_t ReciveTimeout)
{
	for (uint8_t i = 0; i < RECIVING_PIPES_MAXCOUNT; i++)
	{
		if (reciving_pipes_state[i])
		{
			DBGPRINT("Waiting pipe %d", i);
			*Pipe = reciving_pipes[i];
			
			radio->openReadingPipe(1, *Pipe);
			radio->openWritingPipe(*Pipe);
			radio->startListening();

			long startReciveTime = millis();
			bool rC = false;
			while ( !(rC = radio->available()) && (millis() - startReciveTime) < PACKET_TIMEOUT ) ;

			if (rC)
			{
				DBGPRINT("Start reciving, pipe %d", i);
				radio->read(buf, 32);
				
				ProtocolMessage *PM = (ProtocolMessage*)buf;
				radio->stopListening();
				radio->write(&(PM->CurrentNum), sizeof(uint8_t));
				
				//radio->writeAckPayload(1, &(PM.CurrentNum), sizeof(uint8_t));

				if (PM->CurrentNum != 0)
					continue;

				
				DBGPRINT("Package count %d", PM->AllLength);
				

				*size = PM->AllLength*30;
				if (*size > MaxSize)
					return ERROR_NO_ENAUGHT_MEMORY;

				DBGPRINT("Memory copy");
			    memcpy(data, PM->data, 30);

				DBGPRINT("Data size %d", *size);
				long lastSuccRecv = millis();
				for (uint8_t j = 1; j < PM->AllLength; j++)
				{


					if (millis() - lastSuccRecv > ReciveTimeout)
					{
						DBGPRINT("Package %d timeout", PM->CurrentNum);
						return ERROR_TIMEOUT;
					}

					startReciveTime = millis();
					radio->startListening();
					while ( !(rC = radio->available()) && (millis() - startReciveTime) < PACKET_TIMEOUT ) ;
			
					if (rC)
					{
						radio->read(buf, 32);
						radio->stopListening();
						PM = (ProtocolMessage*)buf;
						
						if (PM->CurrentNum < j)
						{
							bool ackres = radio->write(&(PM->CurrentNum), sizeof(uint8_t));
							j--;
							DBGPRINT("Duplicate packet %d, ackres: %d", PM->CurrentNum, ackres);
							continue;
						}
						else if (PM->CurrentNum > j)
						{
							DBGPRINT("Radio transmitt error on package %d", PM->CurrentNum);
							return ERROR_RADIO_TRANSMITT;
						}


						
						radio->write(&j, sizeof(uint8_t));
						memcpy(&(data[j*30]), PM->data, 30);
						bin_dump((char *)(PM->data), 30);
						lastSuccRecv = millis();
						DBGPRINT("Recived packege %d", j);
					}

				}

				

					while ( !(rC = radio->available()) && (millis() - startReciveTime) < PACKET_TIMEOUT ) 

						radio->write(&(PM->CurrentNum), sizeof(uint8_t));
			



				DBGPRINT("Recive complete");
				return ERROR_SUCCESS;
			}
			else
			{
				DBGPRINT("No data");
				continue;
			}
			
		}
	}

	return ERROR_NO_ACTIVE_PIPE;
}