122 lines
2.5 KiB
C
122 lines
2.5 KiB
C
//
|
|
// Created by nano on 4/26/25.
|
|
//
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include "utils.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
#include "communication.h"
|
|
|
|
#define ACK 16
|
|
#define NOTHING_RECV 0
|
|
|
|
/**Helper functions to convert from bits to bytes*/
|
|
void byte_to_bit_array(byte bit_array[], byte data) {
|
|
for (int i = 0; i < 8; i++) {
|
|
bit_array[i] = (data >> i) & 1; //shift through all bits and check if they're 1
|
|
}
|
|
}
|
|
|
|
/**2's complement notation*/
|
|
void bit_array_to_byte(const byte bit_array[], byte *data) {
|
|
*data = 0;
|
|
byte currentpow = 1;
|
|
for (int i = 0; i < 8; i++) {
|
|
*data += bit_array[i] * currentpow;
|
|
currentpow *= 2;
|
|
}
|
|
}
|
|
|
|
void comm_write_until_successful(byte d, int timeout) {
|
|
//while (comm_await_comm_awaready_for_tx(50)!=SP_OK) {} //wait indefinitely till you can send data
|
|
REWRITE:
|
|
comm_blocking_write(&d,1,timeout);
|
|
|
|
byte data;
|
|
int counter =0;
|
|
REREAD:
|
|
while (comm_blocking_read(&data,1,timeout) != ALL_BYTES) {counter ++; if (counter == 10) goto REWRITE;} //idk if this does anything but the code works so im not touching it
|
|
|
|
//data ? printf("RECV: %d \n",data) : 0;
|
|
counter++;
|
|
printf("Times Read %d",counter);
|
|
if (data == ACK) {
|
|
printf("ACK\n");
|
|
}
|
|
else if (counter !=10)
|
|
goto REREAD;
|
|
else
|
|
goto REWRITE;
|
|
}
|
|
|
|
#define CROSSING 0x20
|
|
|
|
/**
|
|
* await data from the robot
|
|
* @param data data gotten from robot
|
|
* @return 1 if correct, -1 if retry required
|
|
*/
|
|
int comm_await_crossing(int timeout){
|
|
byte data;
|
|
int run =1;
|
|
printf("AWAITING CROSSING\n");
|
|
while (run){
|
|
comm_blocking_read(&data, 1,timeout);
|
|
printf("Read while expecting crossing: %d\n",data);
|
|
if (data==CROSSING) {
|
|
//printf("CROSSING\n");
|
|
run=0;
|
|
return 1;
|
|
break;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
#define SENSOR_NOTHING_AHEAD 5
|
|
#define SENSOR_1_CROSSING_AHEAD 1
|
|
#define SENSOR_2_CROSSING_AHEAD 2
|
|
#define SENSOR_3_CROSSING_AHEAD 3
|
|
#define SENSOR_4_CROSSING_AHEAD 4
|
|
|
|
int comm_get_distance_sensor(int *len, int timeout) {
|
|
byte *data = malloc(sizeof(byte));
|
|
while (comm_blocking_read(data,1,timeout) == ALL_BYTES) {
|
|
printf("Got while expecting distance sensor: %d \n", *data);
|
|
switch (*data) {
|
|
case SENSOR_NOTHING_AHEAD:
|
|
*len=0;
|
|
free(data);
|
|
return 1;
|
|
break;
|
|
case SENSOR_1_CROSSING_AHEAD:
|
|
*len=1;
|
|
free(data);
|
|
return 1;
|
|
break;
|
|
case SENSOR_2_CROSSING_AHEAD:
|
|
*len=3;
|
|
free(data);
|
|
return 1;
|
|
break;
|
|
case SENSOR_3_CROSSING_AHEAD:
|
|
*len=5;
|
|
free(data);
|
|
return 1;
|
|
break;
|
|
case SENSOR_4_CROSSING_AHEAD:
|
|
*len=7;
|
|
free(data);
|
|
return 1;
|
|
break;
|
|
default:
|
|
printf("wat\n");
|
|
break;
|
|
}
|
|
}
|
|
free(data);
|
|
return 0;
|
|
} |