struct state simulate_with_controller(struct state init_state, double time){
struct state state_x;
state_x = init_state;
int steps = time/SIM_STEP;
struct controller_storage cs;
cs.int_elevation = 0;
cs.int_pitch = 0;
cs.int_travel = 0;
int k = 0;
for (k = 0; k <steps; k++){
struct command U = controller_safety(sps, state_x, &cs);
state_x = eval_state(state_x, U);
if ( check_safety(state_x) == 0){
state_x.safe = 0;
printf("sim2 nsafe: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf\n", state_x.elevation, state_x.pitch, state_x.travel, state_x.d_elevation, state_x.d_pitch, state_x.d_travel);
return state_x;
}
}
state_x.safe = 1;
printf("sim2 safe: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf\n", state_x.elevation, state_x.pitch, state_x.travel, state_x.d_elevation, state_x.d_pitch, state_x.d_travel);
return state_x;
}
int main()
{
int File_Descriptor, i, j; /**/
int Return_Value ; /*To catch the return value .......*/
char Buffer[Q8_EXAMPLE2_MAXLEN] ;
int tmp;
/* .................Opening the device (Read/Write)................*/
int err = pthread_create(&tid, NULL, &get_keyboard, NULL);
if (err != 0)
printf("second thread not created\n");
else
printf("thread created\n");
printf("\n\tOpening the device ..please wait...\n");
fflush(stdout);
File_Descriptor = open(DEVICE_FILE_NAME, O_RDWR);
if( File_Descriptor < 0 ) /*Display a message if any error occures*/
{
perror("open");
fprintf( stderr, "\n\tCould not open device file - Error : %i\n", File_Descriptor );
return -1;
}
//ioctl(File_Descriptor, Q8_IOCTL_EXAMPLE1);
//ioctl(File_Descriiptor, Q8_IOCTL_EXAMPLE2, Buffer);
//for (i = 0; i < Q8_EXAMPLE2_MAXLEN; i ++)
//printf ("%c", Buffer[i]);
//printf ("\n");
//ioctl(File_Descriptor, Q8_IOCTL_EXAMPLE3, 5);
//ioctl(File_Descriptor, Q8_RD_ENC1, &tmp);
//printf("ENC1 = %x \n", tmp);
/* .. Writing the control values to the left and write motor......*/
int step = 0;
double vol_step = 0.1;
FILE *ofp;
ofp = fopen("recorded_data.txt", "w");
int sensor_readings[3];
err = ioctl(File_Descriptor, Q8_ENC, sensor_readings);
if(err != 0)
{
perror("Epic Fail first enc read\n");
return -1;
}
int base_travel = sensor_readings[0];
int base_pitch = sensor_readings[1];
int base_elevation = sensor_readings[2];
double d_travel = 0;
double d_pitch = 0;
double d_elevation = 0;
struct controller_storage storage_safety;
storage_safety.int_travel = 0;
storage_safety.int_pitch = 0;
storage_safety.int_elevation = 0;
struct controller_storage storage_complex;
storage_complex.int_travel = 0;
storage_complex.int_pitch = 0;
storage_complex.int_elevation = 0;
struct controller_storage storage; //for the current loop
struct state spc; //set point for safety and complex controllers
int remaining_safety_cycles = 0 ;
for (step = 0 ; step < 15000 ; step++){
unsigned short int tmparray[4];
tmparray[0] = Q8_dacVTO((vol_right), 1, 10);
tmparray[1] = Q8_dacVTO((vol_left), 1, 10);
ioctl(File_Descriptor, Q8_WR_DAC, tmparray);
/* .. Reading the Encoder values from the helicopter......*/
err = ioctl(File_Descriptor, Q8_ENC, sensor_readings);
if(err != 0)
{
perror("Epic Fail first enc read\n");
return -1;
}
int travel = sensor_readings[0] - base_travel;
int pitch = sensor_readings[1] - base_pitch;
int elevation = -(sensor_readings[2] - base_elevation)-300;
struct state cs;
cs.elevation = 1.0/10.0 * 3.1415/180.0 * (double) elevation;
cs.pitch = 90.0/1000.0 * 3.1415/180.0 * (double) pitch;
cs.travel = 3.1415/4089.00 * (double) travel;
cs.d_travel = (cs.travel - storage.travel1)/PERIOD;
cs.d_pitch = (cs.pitch - storage.pitch1)/PERIOD;
cs.d_elevation = (cs.elevation - storage.elevation1)/PERIOD;
storage.elevation2 = storage.elevation1;
storage.elevation1 = cs.elevation;
storage.pitch2 = storage.pitch1;
storage.pitch1 = cs.pitch;
storage.travel2 = storage.travel1;
storage.travel1 = cs.travel;
printf("\n step: %d, sp.travel = %lf, travel = %lf, pitch = %lf, elevation = %lf, left: %lf, right: %lf\n", step, spc.travel, cs.travel, cs.pitch , cs.elevation, vol_left, vol_right);
if (record == 1){
fprintf(ofp, "%d\t%d\t%d\t%d\t%lf\t%lf\n", step, travel, pitch, elevation, vol_right, vol_left);
}
if (step < 30 ){
vol_right = 0;
vol_left = 0;
}
else
{
/*
The logic is that after every restart the safety controller is active for a set amount of time. After that if the
complex controller's command is safe, it can be used again.
*/
if (step > 1800)
{
spc.travel = 3.1415/2;
}
else if( step > 1600){
spc.travel = 3.1415/4;
}
else if (step > 1400){
spc.travel = 0;
}
else if (step > 900){
spc.travel = 3.14;
}
else if (step > 400){
spc.elevation = 0.4;
spc.travel = 3.14/2;
}
else if (step > 300){
sps.elevation = 0.4;
}
else if (step > 200){
sps.elevation = 0.3;
}
else if (step > 150){
sps.elevation = 0.2;
}
else if (step > 100){
sps.elevation = 0.1;
}
else if (step >50){
sps.elevation = 0;
}
struct command U_safety = controller_safety(sps, cs, &storage_safety);
struct command U_complex = controller_complex(spc, cs, &storage_complex);
printf("remaining_cycle %d\n", remaining_safety_cycles);
if (step < 400){
vol_right = U_safety.u1;
vol_left = U_safety.u2;
}
else if(decide(cs, U_complex, 0.2) == 1 && (remaining_safety_cycles <= 0 ) ) {
printf("complex controller\n");
vol_right = U_complex.u1;
vol_left = U_complex.u2;
}
else {
printf("safety controller\n");
vol_right = U_safety.u1;
vol_left = U_safety.u2;
remaining_safety_cycles -= 1;
}
}
if(step % 200 == 0){
printf("restart\n");
usleep(RESTART_TIME *1000000.0);
remaining_safety_cycles = 60;
}
// vol_right = 0;
// vol_left = 0;
// printf ("elev -1/3* pitch : %lf elev + 1.0/3.0*pitch: %lf\n", cs.elevation-0.3333*cs.pitch, cs.elevation + 0.3333*cs.pitch);
if (vol_right > MAX_VOLTAGE )
vol_right = MAX_VOLTAGE;
else if (vol_right < -MAX_VOLTAGE)
vol_right = -MAX_VOLTAGE;
if (vol_left > MAX_VOLTAGE)
vol_left = MAX_VOLTAGE;
else if (vol_left < -MAX_VOLTAGE)
vol_left = -MAX_VOLTAGE;
vol_right = voltage_max_min(vol_right);
vol_left = voltage_max_min(vol_left);
usleep ((int) (PERIOD * 1000000.0));
}
fclose(ofp);
return 0;
}
int main()
{
struct sched_param param;
param.sched_priority = 97;
if(sched_setscheduler(0, SCHED_FIFO, ¶m) == -1)
{
perror("sched_setscheduler failed");
exit(-1);
}
/* Lock memory */
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
perror("mlockall failed");
exit(-2);
}
/* Pre-fault our stack */
stack_prefault();
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
t.tv_sec++;
char *dev;
dev = WATCHDOGDEV;
int fd = open(dev, O_RDWR);
int interval=1;
int n = 0;
struct sockaddr_in serv_addr;
char *message ;
int num;
signal(SIGPIPE, SIG_IGN);
signal(SIGINT,Sa_handler);
char recvBuff[68];
char sendBuff[68];
memset(recvBuff, '0',sizeof(recvBuff));
if((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
{
printf("\n Error : Could not create socket \n");
return -1 ;
}
memset(&serv_addr, '0', sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(5000);
if(inet_pton(AF_INET,"192.168.7.1", &serv_addr.sin_addr)<=0)
{
printf("\n inet_pton error occured\n");
return -1;
}
if( connect(sockfd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
{
printf("\n Error : Connect Failed \n");
return -1;
}
if (ioctl(fd, WDIOC_SETTIMEOUT, &interval) != 0)
{
fprintf(stderr,"Error: Set watchdog interval failed\n");
exit(EXIT_FAILURE);
}
while(1)
{
ioctl(fd, WDIOC_KEEPALIVE, NULL);
fprintf(stdout, "Kick watchdog through IOCTL\n");
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
struct state st;
signal(SIGINT,Sa_handler);
int k,g;
memset(recvBuff, '0', sizeof(recvBuff));
if((num = recv(sockfd, &k, sizeof(k),0))<=0)
{
printf("Either Connection Closed or Error\n");
close(sockfd);
exit(1);
}
printf("k=%d \n",k);
if((num = recv(sockfd, recvBuff, k,0))<=0)
{
printf("Either Connection Closed or Error\n");
close(sockfd);
exit(1);
}
recvBuff[num] = '\0';
printf("Client:Message Received From Server - %s\n",recvBuff);
sscanf(strtok(recvBuff , ", "), "%lf", &st.elevation);
sscanf(strtok(NULL, ", "), "%lf",&st.pitch);
sscanf(strtok(NULL, ", "), "%lf",&st.travel);
sscanf(strtok(NULL, ", "), "%lf",&st.d_elevation);
sscanf(strtok(NULL, ", "), "%lf",&st.d_pitch);
sscanf(strtok(NULL, ", "), "%lf",&st.d_travel);
printf("client:state received \n");
printf("elevation %lf\n",st.elevation);
printf("pitch %lf\n",st.pitch);
printf("travel %lf\n",st.travel);
printf("d_elevation %lf\n",st.d_elevation);
printf("d_pitch %lf\n",st.d_pitch);
printf("d_travel %lf\n",st.d_travel);
struct command comm= controller_safety(st);
printf(" u1= %d u2= %d \n",comm.u1,comm.u2);
sprintf(sendBuff,"complex, %f, %f, ",comm.u1,comm.u2);
g=strlen(sendBuff);
if ((send(sockfd,&g,sizeof(int),0))== -1)
{
fprintf(stderr, "Failure Sending Message\n");
close(sockfd);
break;
}
if((send(sockfd,sendBuff,g,0))==-1)
{
printf("Failure in sending Message\n");
close(sockfd);
exit(1);
}
t.tv_nsec += interval;
while (t.tv_nsec >= NSEC_PER_SEC)
{
t.tv_nsec -= NSEC_PER_SEC;
t.tv_sec++;
}
}
close(sockfd);
return -1;
}
