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;
}
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;
}
