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