int main(void) {
NU32_Startup();
//NU32_WriteUART1("HelloWorld\n");
char buf[MESSAGE_MAX];
int c='0';
char state='x';
int n=0;
//allocate struct array of max size
//struct args control_args = (struct args)malloc(N_MAX * sizeof(struct args));
struct args control_args[N_MAX];
while (1) {
NU32_WriteUART1("Press w to write, r to read, and x to swing\r\n");
NU32_ReadUART1(buf,MESSAGE_MAX);
c = buf[0];
if(c=='w'){
NU32_WriteUART1("Input n: \r\n");
NU32_ReadUART1(buf, MESSAGE_MAX);
sscanf(buf,"%d", &n);
write_swing(n, control_args);
}
else if(c=='r'){
read_swing(n,control_args);
}
else if(c=='x'){
//swing robot
}
}
//free(control_args);
}
// Send the motor gains to the client
void motor_gains_write() {
char buffer[200];
sprintf(buffer,"\%f \r\n", Kp);
NU32_WriteUART1(buffer);
sprintf(buffer,"\%f \r\n", Ki);
NU32_WriteUART1(buffer);
}
void read_swing(int n, struct args* control_args){
char message[MESSAGE_MAX];
int i;
sprintf(message, " Torque Swing(ms) Fly(ms)\r\n");
NU32_WriteUART1(message);
//print entire struct array
for (i=0; i<n; i++){
sprintf(message, "Set %d: %6d %9d %8d \r\n",i+1,control_args[i].torque,control_args[i].t_swing,control_args[i].t_flight);
NU32_WriteUART1(message);
}
NU32_WriteUART1("\r\n");
}
void configure_receiver(){
unsigned char status;
CE = 0;
status = radio_write_register(0x20, 0x39); //PRX, CRC enabled
sprintf(msg,"%d \r\n", status);
NU32_WriteUART1("Status 1: ");
NU32_WriteUART1(msg);
status = radio_write_register(0x21, 0x00); //disable auto-ack for all channels
sprintf(msg,"%d \r\n", status);
NU32_WriteUART1("Status 2: ");
NU32_WriteUART1(msg);
status = radio_write_register(0x23, 0x03); // address width = 5
sprintf(msg,"%d \r\n", status);
NU32_WriteUART1("Status 3: ");
NU32_WriteUART1(msg);
radio_write_register(0x26, 0x07); // data rate = 1MB
radio_write_register(0x31, 0x04); // 4 byte payload
radio_write_register(0x25, 0x02); // set channel 2, this is default
radio_write_register(0x30, 0xE7); // set address E7E7E7E7E7
radio_write_register(0x20, 0x3B); // Power up to change to change to STAND BY state
CE = 1;
int counter;
for (counter = 0; counter < 80000; counter++){;}
status = radio_command(0xFF);
sprintf(msg,"%d \r\n", status);
NU32_WriteUART1("Status 4: ");
NU32_WriteUART1(msg);
}
void write_swing(int n, struct args* control_args){
//Note: this function will be condensed after testing
//get input from user and send to the PIC
int torque, t_swing, t_flight;
char torque_buf[10], swing_buf[10], fly_buf[10];
char message[MESSAGE_MAX];
//struct args control_args[n];
int i;
for (i=0; i<n; i++){
sprintf(message, "Enter data for set %d\r\n", i+1);
NU32_WriteUART1(message);
//Torque
NU32_WriteUART1("Torque: ");
NU32_ReadUART1(torque_buf, 10);
sscanf(torque_buf, "%d", &torque);
sprintf(message, "%d \r\n", torque);
NU32_WriteUART1(message);
control_args[i].torque = torque;
//Time swinging
NU32_WriteUART1("Time to swing: ");
NU32_ReadUART1(swing_buf, 10);
sscanf(swing_buf, "%d", &t_swing);
sprintf(message, "%d \r\n", t_swing);
NU32_WriteUART1(message);
control_args[i].t_swing = t_swing;
//Time in flight
NU32_WriteUART1("Time to fly: ");
NU32_ReadUART1(fly_buf,10);
sscanf(fly_buf, "%d", &t_flight);
sprintf(message, "%d \r\n\n", t_flight);
NU32_WriteUART1(message);
control_args[i].t_flight = t_flight;
}
return 0;
}