// approach control, void approach_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){ double phi = navData_ptr->phi; // roll angle double theta = navData_ptr->the - base_pitch_cmd; // subtract theta trim value to convert to delta coordinates double p = sensorData_ptr->imuData_ptr->p; // roll rate double q = sensorData_ptr->imuData_ptr->q; // pitch rate double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed double ias_cmd, phi_cmd, theta_cmd; controlData_ptr->theta_cmd = approach_theta - base_pitch_cmd; // delta theta command [rad] controlData_ptr->ias_cmd = approach_speed; // absolute airspeed command [m/s] controlData_ptr->phi_cmd = pilot_phi(sensorData_ptr); // phi from the pilot stick theta_cmd = controlData_ptr->theta_cmd; ias_cmd = controlData_ptr->ias_cmd; phi_cmd = controlData_ptr->phi_cmd; controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1] controlData_ptr->l1 = 0; // L1 [rad] controlData_ptr->l2 = roll_control(phi_cmd, phi, p, TIMESTEP, 0); // L2 [rad] controlData_ptr->l3 = pitch_control(theta_cmd, theta, q, TIMESTEP, 0); // L3 [rad] controlData_ptr->l4 = controlData_ptr->l3 + controlData_ptr->l2; // L4 [rad] controlData_ptr->r1 = 0; // R1 [rad] controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad] controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad] controlData_ptr->r4 = controlData_ptr->r3 + controlData_ptr->r2; // R4 [rad] }
void pilot_flying_inner(double time, double ias_cmd, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){ double phi = navData_ptr->phi; // roll angle double theta = navData_ptr->the - base_pitch_cmd; // subtract theta trim value to convert to delta coordinates double p = sensorData_ptr->imuData_ptr->p; // roll rate double q = sensorData_ptr->imuData_ptr->q; // pitch rate double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed double phi_cmd, theta_cmd; controlData_ptr->phi_cmd = pilot_phi(sensorData_ptr) + controlData_ptr->phi_cmd; // phi from the pilot stick + guidance controlData_ptr->theta_cmd = pilot_theta(sensorData_ptr) + controlData_ptr->theta_cmd; // theta from the pilot stick + guidance controlData_ptr->ias_cmd = ias_cmd; theta_cmd = controlData_ptr->theta_cmd; phi_cmd = controlData_ptr->phi_cmd; controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1] controlData_ptr->l1 = 0; // L1 [rad] controlData_ptr->l2 = roll_control(phi_cmd, phi, p, TIMESTEP, 1); // L2 [rad] controlData_ptr->l3 = pitch_control(theta_cmd, theta, q, TIMESTEP, 1); // L3 [rad] controlData_ptr->l4 = 0; // L4 [rad] controlData_ptr->r1 = 0; // R1 [rad] controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad] controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad] controlData_ptr->r4 = 0; // R4 [rad] }
void flare_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){ double ramp_time = 6.0; double min_speed = 8; double cut_time = 5; double phi = navData_ptr->phi; // roll angle double theta = navData_ptr->the - base_pitch_cmd; // subtract theta trim value to convert to delta coordinates double p = sensorData_ptr->imuData_ptr->p; // roll rate double q = sensorData_ptr->imuData_ptr->q; // pitch rate double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed static int t1_latched = FALSE; // time latching static double t1 = 0; double ias_cmd, phi_cmd, theta_cmd; if(time <= ramp_time){ // ramp commands ias_cmd = approach_speed - time*((approach_speed - flare_speed)/ramp_time); theta_cmd = approach_theta + time*((flare_theta - approach_theta)/ramp_time); } else{ // final values ias_cmd = flare_speed; theta_cmd = flare_theta; } if(ias < min_speed){ // engine cutoff if(t1_latched == FALSE){ t1 = time; t1_latched = TRUE; } if((time-t1) > cut_time){ ias_cmd = -100; } } else{ t1_latched = FALSE; } controlData_ptr->theta_cmd = theta_cmd - base_pitch_cmd; // delta theta command [rad] controlData_ptr->ias_cmd = ias_cmd; // absolute airspeed command [m/s] controlData_ptr->phi_cmd = 0; // wings level theta_cmd = controlData_ptr->theta_cmd; ias_cmd = controlData_ptr->ias_cmd; phi_cmd = controlData_ptr->phi_cmd; controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1] controlData_ptr->l1 = 0; // L1 [rad] controlData_ptr->l2 = roll_control(phi_cmd, phi, p, TIMESTEP, 0); // L2 [rad] controlData_ptr->l3 = pitch_control(theta_cmd, theta, q, TIMESTEP, 0); // L3 [rad] controlData_ptr->l4 = controlData_ptr->l3 + controlData_ptr->l2; // L4 [rad] controlData_ptr->r1 = 0; // R1 [rad] controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad] controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad] controlData_ptr->r4 = controlData_ptr->r3 + controlData_ptr->r2; // R4 [rad] }
void open_loop(double time, double ias_cmd, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){ double ias = sensorData_ptr->adData_ptr->ias_filt; // filtered airspeed double roll_incp = sensorData_ptr->inceptorData_ptr->roll; double pitch_incp = sensorData_ptr->inceptorData_ptr->pitch; controlData_ptr->ias_cmd = ias_cmd; controlData_ptr->dthr = speed_control(ias_cmd, ias, TIMESTEP); // Throttle [ND 0-1] controlData_ptr->l1 = 0; // L1 [rad] controlData_ptr->l2 = roll_incp*L2_MAX; // L2 [rad] controlData_ptr->l3 = -1*pitch_incp*(L3_MAX-15*D2R); // L3 [rad] controlData_ptr->l4 = 0; // L4 [rad] controlData_ptr->r1 = 0; // R1 [rad] controlData_ptr->r2 = -1*controlData_ptr->l2; // R2 [rad] controlData_ptr->r3 = controlData_ptr->l3; // R3 [rad] controlData_ptr->r4 = 0; // R4 [rad] }
void odom_callback(const nav_msgs::Odometry odom_ref){ std::cout<<"segundos: "<<odom_ref.header.stamp.sec<<" nanosegundos: "<<odom_ref.header.stamp.nsec<<"--> en segundos: "<<(float)odom_ref.header.stamp.nsec/1000000000<<std::endl; double tiempo = (double)odom_ref.header.stamp.sec+(double)odom_ref.header.stamp.nsec/1000000000; double error_temp = tiempo-t_prev_loop; std::cout<<"Tiempo entre actualizaciones: "<<error_temp<<std::endl; odom_rv = odom_ref.twist.twist; std::cout<<"------------------------------"<<std::endl; std::cout<<"ODOM UPDATE"<<std::endl; //std::cout<<odom_ref<<std::endl; std::cout<<"ODOM POSITION"<<std::endl; std::cout<<odom_ref.pose.pose.position<<std::endl; std::cout<<"ODOM VEL"<<std::endl; std::cout<<"("<<odom_rv.linear<<", "<<odom_rv.angular.z<<" )"<<std::endl; std::cout<<"elemento vector"<<ref_pos+1<<std::endl; //TODO //std_position(odom_ref); // std::cout<<"FLYING STATE"<<std::endl; // std::cout<<bebop_fst<<std::endl; float e_z,e_x,e_y, z_ref,x_ref, y_ref; x_ref=vec_ref.at(ref_pos).x; y_ref=vec_ref.at(ref_pos).y; z_ref=vec_ref.at(ref_pos).z; std::cout<<"REFERENCIA: ("<<x_ref<<", "<<y_ref<<", "<<z_ref<<" )"<<std::endl; e_x=x_ref-odom_ref.pose.pose.position.x; e_y=y_ref-odom_ref.pose.pose.position.y; //geometry_msgs::Twist twistter; e_z=z_ref-odom_ref.pose.pose.position.z; if (flag_fst==true){ std::cout<<"SPEED_CONTROL ACTIVATED"<<std::endl; if(ref_pos<vec_ref.size()-1){ if(/*(e_y<=0.1)&&*/(e_x<=0.1)){ ref_pos++; } } speed_control(e_x,e_y,e_z,(float)error_temp); } t_prev_loop=tiempo; }
int main() { int sockfd; float brightness; struct sockaddr_in server_addr; char response[MAX_SIZE]; char cmd[MAX_SIZE]; char cmd2[MAX_SIZE]; FILE *fp, *fp2; char component_string[MAX_SIZE]; char value_string[MAX_SIZE]; int value_int; mraa_pwm_context speed_pwm_in1, speed_pwm_in2, turn_pwm; speed_pwm_in1 = mraa_pwm_init(3); speed_pwm_in2 = mraa_pwm_init(5); turn_pwm = mraa_pwm_init(6); if (speed_pwm_in1 == NULL || speed_pwm_in2 == NULL || turn_pwm == NULL) { fprintf(stderr, "Failed to initialized.\n"); return 1; } mraa_pwm_period_us(speed_pwm_in1,870); //1150Hz mraa_pwm_enable(speed_pwm_in1, 1); mraa_pwm_period_us(speed_pwm_in2,870); mraa_pwm_enable(speed_pwm_in2, 1); mraa_pwm_period_ms(turn_pwm,20); mraa_pwm_enable(turn_pwm, 1); mraa_pwm_write(speed_pwm_in1, 1.0f); mraa_pwm_write(speed_pwm_in2, 1.0f); mraa_pwm_write(turn_pwm, CENTER); //Set the protocol to mqtt and //restart iotkit-agent to ensure that it is running. system("iotkit-admin protocol mqtt"); system("systemctl restart iotkit-agent"); //create a socket with the following attributes: socklen_t len = sizeof(server_addr); sockfd = socket(AF_INET, SOCK_DGRAM, 0); bzero(&server_addr,sizeof(server_addr)); //Name of the socket as agreed with the server server_addr.sin_family = AF_INET; server_addr.sin_addr.s_addr = inet_addr(localhost); server_addr.sin_port = htons(41235); if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) { fprintf(stderr, "Failed to bind"); return 1; } while(1) { //Use recvfrom function to send JSON message to iotkit-agent if(recvfrom(sockfd, response, MAX_SIZE, 0, (struct sockaddr *)&server_addr, &len) < 0) { fprintf(stderr, "Failed to receive actuation\n"); return 1; } //printf("Received JSON message: %s\n", response); //---This part extracts the component name---------------- snprintf(cmd, sizeof(cmd), "echo '%s' | sed 's/\"/ /g' | tr ' ' '\n' | awk '/component/{getline; getline; print}'", response); fp = popen(cmd, "r"); if (fp == NULL) { fprintf(stderr, "Failed to run command\n" ); exit(1); } while (fgets(component_string, sizeof(component_string)-1, fp) != NULL) { } pclose(fp); //---This part extracts the value------------------------- snprintf(cmd2, sizeof(cmd2), "echo '%s' | sed 's/\"/ /g' | tr ' ' '\n' | awk '/value/{getline; getline; print}'", response); fp2 = popen(cmd2, "r"); if (fp2 == NULL) { fprintf(stderr, "Failed to run command\n" ); exit(1); } while (fgets(value_string, sizeof(value_string)-1, fp) != NULL) { } pclose(fp2); //printf(value_string); //---This part is for actuation if (!strcmp(component_string, "vehicleControl\n")) { if (!strcmp(value_string, "forward\n")) { printf("Going forward for 1 second.\n\n"); mraa_pwm_write(turn_pwm, CENTER); //straight usleep(100000); speed_control(speed_pwm_in1, speed_pwm_in2, 100); sleep(1); speed_control(speed_pwm_in1, speed_pwm_in2, 0); } else if (!strcmp(value_string, "reverse\n")) { printf("Going backward for 1 second.\n\n"); mraa_pwm_write(turn_pwm, CENTER); //reverse usleep(100000); speed_control(speed_pwm_in1, speed_pwm_in2, -100); sleep(1); speed_control(speed_pwm_in1, speed_pwm_in2, 0); } else if (!strcmp(value_string, "left\n")) { printf("Making a left turn.\n\n"); mraa_pwm_write(turn_pwm, CENTER - 0.015f); //left usleep(100000); speed_control(speed_pwm_in1, speed_pwm_in2, 100); sleep(1); speed_control(speed_pwm_in1, speed_pwm_in2, 0); } else if (!strcmp(value_string, "right\n")) { printf("Making a right turn.\n\n"); mraa_pwm_write(turn_pwm, CENTER + 0.015f); //right usleep(100000); speed_control(speed_pwm_in1, speed_pwm_in2, 100); sleep(1); speed_control(speed_pwm_in1, speed_pwm_in2, 0); } else fprintf(stderr, "Wrong actuation command received!\n"); } } close(sockfd); return 0; }
int main(){ /**********************************************************************/ /**********************************************************************/ /* Begin. */ mraa_uart_context uart; uart = mraa_uart_init(0); mraa_uart_set_baudrate(uart, 115200); char buffer[] = "hhh"; char flush[]="xxxxxxxxxxxx"; char str[] = "HELLO"; if (uart == NULL) { fprintf(stderr, "UART failed to setup\n"); printf("UART failed"); return 1; } printf("firstavail:%d\n",mraa_uart_data_available(uart,0)); while (mraa_uart_data_available(uart,0)) { mraa_uart_read(uart, flush, sizeof(uart)); printf("Flush: %c %c %c %c %c %c %c %c",flush[0], flush[1], flush[2],flush[3],flush[4],flush[5],flush[6],flush[7]); usleep(150); } printf("available: %d",mraa_uart_data_available(uart,0)); char speed_user_input[MAXBUFSIZ]; char turn_user_input[MAXBUFSIZ]; mraa_pwm_context speed_pwm_in1, speed_pwm_in2, turn_pwm; speed_pwm_in1 = mraa_pwm_init(3); speed_pwm_in2 = mraa_pwm_init(5); turn_pwm = mraa_pwm_init(6); if (speed_pwm_in1 == NULL || speed_pwm_in2 == NULL || turn_pwm == NULL) { fprintf(stderr, "Failed to initialized.\n"); return 1; } mraa_pwm_period_us(speed_pwm_in1,870); //1150Hz mraa_pwm_enable(speed_pwm_in1, 1); mraa_pwm_period_us(speed_pwm_in2,870); mraa_pwm_enable(speed_pwm_in2, 1); mraa_pwm_period_ms(turn_pwm,20); mraa_pwm_enable(turn_pwm, 1); mraa_pwm_write(turn_pwm, CENTER); mraa_pwm_write(speed_pwm_in1, 1.0f); mraa_pwm_write(speed_pwm_in2, 1.0f); int n = 5; int spd = 0; char direction = 'C'; char cross_clr = 'B'; float angle = 0.0f; while (1) { //sleep(2); //readCharAry(uart,flush); spd = 50; mraa_uart_read(uart, buffer, 1); if(buffer[0]=='\n') { printf("new line "); mraa_uart_read(uart, buffer, 1); if(buffer[0]=='\n') { printf("new line "); mraa_uart_read(uart, buffer, 1); } } mraa_uart_read(uart, buffer+1, 1); if(buffer[1]=='\n') { buffer[0] = 'h'; buffer[1] = 'h'; } int sign = 0; if(cross_clr == 'M') { speed_control(speed_pwm_in1, speed_pwm_in2, 0.0f); sleep(1); char* nearestBeaconID = (char*)malloc(5); getStrongestBeacon(nearestBeaconID); printf("the nearestBeaconID is: %s\n", nearestBeaconID); sign = 1; angle = CENTER + 0.015f; mraa_pwm_write(turn_pwm, angle); usleep(15000); speed_control(speed_pwm_in1, speed_pwm_in2, 54); printf("speed: %d",spd); usleep(5500000); speed_control(speed_pwm_in1, speed_pwm_in2, 0.0f); //sleep(1); mraa_uart_write(uart, str, 1); //while(!mraa_uart_data_available(uart, 10)){}; mraa_uart_read(uart, buffer, 1); while(buffer[0] == '\n') { mraa_uart_read(uart, buffer, 1); } if(buffer[0] == '\0') direction = 'C'; mraa_uart_read(uart, buffer + 1, 1); cross_clr = buffer[1]; } printf("buff:%c %c %c \n",buffer[0], buffer[1],buffer[2]); if(!sign){ if (direction == 'L') angle = CENTER - 0.005f; if (direction == 'R') angle = CENTER + 0.005f; if (direction == 'C') angle = CENTER;} else { if (direction == 'C') angle = CENTER +0.013f; if (direction == 'L') angle = CENTER +0.005f; if (direction == 'R') angle = CENTER + 0.019f; } mraa_pwm_write(turn_pwm, angle); speed_control(speed_pwm_in1, speed_pwm_in2, spd); printf("speed: %d",spd); usleep(250000); direction = buffer[0]; cross_clr = buffer[1]; } return 0; }
/*! * Direkter Zugriff auf den Motor * @param left Geschwindigkeit fuer den linken Motor * @param right Geschwindigkeit fuer den linken Motor * Geschwindigkeit liegt zwischen -255 und +255. * 0 bedeutet Stillstand, 255 volle Kraft voraus, -255 volle Kraft zurueck. * Sinnvoll ist die Verwendung der Konstanten: BOT_SPEED_XXX, * also z.B. motor_set(BOT_SPEED_LOW,-BOT_SPEED_LOW); * fuer eine langsame Drehung */ void motor_set(int16 left, int16 right){ #ifdef SPEED_CONTROL_AVAILABLE volatile static int16 old_mot_time_s=0; volatile static int16 old_mot_time_ms=0; #endif if (left == BOT_SPEED_IGNORE) left=BOT_SPEED_STOP; if (right == BOT_SPEED_IGNORE) right=BOT_SPEED_STOP; // Haben wir ueberhaupt etwas zu tun? if ((speed_l == left) && (speed_r == right)){ // Hier sitzt die eigentliche Regelung #ifdef SPEED_CONTROL_AVAILABLE if (timer_get_ms_since(old_mot_time_s,old_mot_time_ms) > SPEED_CONTROL_INTERVAL) { speed_control(); old_mot_time_s= timer_get_s(); old_mot_time_ms= timer_get_ms(); } #endif return; // Keine Aenderung? Dann zuerueck } if (abs(left) > BOT_SPEED_MAX) // Nicht schneller fahren als moeglich speed_l = BOT_SPEED_MAX; else if (left == 0) // Stop wird nicht veraendert speed_l = BOT_SPEED_STOP; else if (abs(left) < BOT_SPEED_SLOW) // Nicht langsamer als die speed_l = BOT_SPEED_SLOW; // Motoren koennen else // Sonst den Wunsch uebernehmen speed_l = abs(left); if (abs(right) > BOT_SPEED_MAX)// Nicht schneller fahren als moeglich speed_r = BOT_SPEED_MAX; else if (abs(right) == 0) // Stop wird nicht veraendert speed_r = BOT_SPEED_STOP; else if (abs(right) < BOT_SPEED_SLOW) // Nicht langsamer als die speed_r = BOT_SPEED_SLOW; // Motoren koennen else // Sonst den Wunsch uebernehmen speed_r = abs(right); if (left < 0 ) speed_l=-speed_l; if (right < 0 ) speed_r=-speed_r; #ifdef SPEED_CONTROL_AVAILABLE // TODO Hier koennten wir die Motorkennlinie heranziehen um gute Einstiegswerte fuer die Regelung zu haben // Evtl. sogar eine im EEPROM kalibrierbare Tabelle?? encoderTargetRateL = left / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf] encoderTargetRateR = right / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf] #endif // Zuerst einmal eine lineare Kennlinie annehmen bot_motor(speed_l/2,speed_r/2); }
void main(void) { int i1=0; int count10S = 0; Device_init(); disable_irq(PIT0_IRQn); systick_delay_ms(50); FLOAT_LDC_init(SPI1); systick_delay_ms(50); FLOAT_LDC_init(SPI0); systick_delay_ms(1000); uart_init(UART3,115200); key_init(0); key_init(1); key_init(2); //LDC_EVM_TEST(); //Speed_Ctl_Init(short point_value, double dKpp, double dKii, double dKdd); while(1) { //value_collect(); LCD_P6x8Str(0,2,"Rmax Lmin"); printf("Rmax Rmin\r\n"); systick_delay_ms(1500); i1=50; while(i1--) { adjustment[2]=(float)filter(SPI1)/10;//右max LCD_BL(55,2,(uint16)adjustment[2]); printf("%d\r\n",adjustment[2]); } i1=50; while(i1--) { adjustment[1]=(float)filter(SPI0)/10;//左min LCD_BL(90,2,(uint16)adjustment[1]); printf("%d\r\n",adjustment[1]); } LCD_BL(55,2,(uint16)adjustment[1]); LCD_BL(90,2,(uint16)adjustment[2]); LCD_P6x8Str(0,4,"Lmax Rmin"); printf("Lmax Rmin\r\n"); systick_delay_ms(1500); i1=50; while(i1--) { adjustment[3]=(float)filter(SPI1)/10;//右min LCD_BL(55,4,(uint16)adjustment[3]); printf("%d\r\n",adjustment[3]); } i1=50; while(i1--) { adjustment[4]=(float)filter(SPI0)/10;//左max LCD_BL(90,4,(uint16)adjustment[4]); printf("%d\r\n",adjustment[4]); } LCD_BL(55,4,(uint16)adjustment[4]); LCD_BL(90,4,(uint16)adjustment[3]); if(adjustment[1]<=adjustment[3]) adjustment[5]=adjustment[1];//次小值 else adjustment[5]=adjustment[3];//次小值 if(adjustment[2]>=adjustment[4]) adjustment[6]=adjustment[2];//次大值 else adjustment[6]=adjustment[4];//次大值 divisor = (adjustment[2] - adjustment[3])/60; //48 //50 divisor2 = (adjustment[4] - adjustment[1])/59; //48 //45 zengfuzhi = (adjustment[2] - adjustment[3])/(adjustment[4] - adjustment[1]); // for(int i=0;i<10;i++) // { // printf("%d\r\n",adjustment[i]); // } // systick_delay_ms(1000); LCD_P6x8Str(0,6,"Mid value"); i1 = 50; systick_delay_ms(1500); while(i1--) { adjustment[7]=(float)filter(SPI0)/10;//左 adjustment[8]=(float)filter(SPI1)/10;//右 LCD_BL(55,6,(uint16)adjustment[7]); LCD_BL(90,6,(uint16)adjustment[8]); } if(adjustment[7]>=adjustment[8]) { flag=1; adjustment[0]=adjustment[7]-adjustment[8]; } else { flag=0; adjustment[0]=adjustment[8]-adjustment[7]; } LCD_Fill(0x00); Out_side_collect(); LCD_Fill(0x00); adjustment_change(); enable_irq(PIT0_IRQn); while(1) { if(PIT_1sFlag == 1) { gpio_turn(PTD4); PIT_1sFlag = 0; count10S ++; } if(key_check(2) == 0) { LCD_Fill(0x00); Motor_stop(); break; } else { if(PIT_5msFlag == 1) { LDC_get(); PIT_5msFlag = 0; Steering_Change(); } if(count10S > 10) { if(gpio_get(PTC4) == 0) { while(1) { Motor_stop(); Steering_Change(); } } } if(PIT_10msFlag == 1) { Quad_count(); now = (float)guad_val; Motor_PID(now); PIT_10msFlag = 0; } if(PIT_20msFlag == 1) { PIT_20msFlag = 0; } if(PIT_50msFlag == 1) { //Result_collect(); PIT_50msFlag = 0; } //printf("guad_val:%d \r\n",(guad_val)); //TEST_display(); speed_control(); //key_choice(); uint16 i23 = gpio_get(PTC4); //LCD_BL(50,2,(uint16)(i23)); } } } }
int main() { float speed, turn; char speed_user_input[MAXBUFSIZ]; char turn_user_input[MAXBUFSIZ]; char time_user_input[MAXBUFSIZ]; mraa_pwm_context speed_pwm_in1, speed_pwm_in2, turn_pwm; speed_pwm_in1 = mraa_pwm_init(3); speed_pwm_in2 = mraa_pwm_init(5); turn_pwm = mraa_pwm_init(6); if (speed_pwm_in1 == NULL || speed_pwm_in2 == NULL || turn_pwm == NULL) { fprintf(stderr, "Failed to initialized.\n"); return 1; } mraa_pwm_period_us(speed_pwm_in1,870); //1150Hz mraa_pwm_enable(speed_pwm_in1, 1); mraa_pwm_period_us(speed_pwm_in2,870); mraa_pwm_enable(speed_pwm_in2, 1); mraa_pwm_period_ms(turn_pwm,20); mraa_pwm_enable(turn_pwm, 1); mraa_pwm_write(turn_pwm, CENTER); mraa_pwm_write(speed_pwm_in1, 1.0f); mraa_pwm_write(speed_pwm_in2, 1.0f); RearWheel *rw = new RearWheel(); //go for the curve thread t1; while(1) { t1 = rw->threading(); printf("Turn (L, C, R): "); scanf("%s", turn_user_input); if (!strcmp(turn_user_input, "L") || !strcmp(turn_user_input, "l")) turn = CENTER - 0.015f; else if (!strcmp(turn_user_input, "C") || !strcmp(turn_user_input, "c")) turn = CENTER; else if (!strcmp(turn_user_input, "R") || !strcmp(turn_user_input, "r")) turn = CENTER + 0.018f; else { printf("Wrong turn type!\n"); return 1; } printf("Speed value (-100-100): "); scanf("%s", speed_user_input); speed = atof(speed_user_input); printf("time: "); scanf("%s", time_user_input); int time = atoi(time_user_input); if (speed > 100 || speed < -100) printf("Error: Choose between -100 and 100\n"); else { mraa_pwm_write(turn_pwm, turn); usleep(100000); speed_control(speed_pwm_in1, speed_pwm_in2, speed); } sleep(time); speed_control(speed_pwm_in1, speed_pwm_in2, 0.0f); float count = rw->getCount(); printf("%.2f \n", count); rw->clear(); t1.join(); } return 0; }