/* get delta angles */ bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const { if (_delta_angle_valid[i]) { delta_angle = _delta_angle[i]; return true; } else if (get_gyro_health(i)) { // provide delta angle from raw gyro, so we use the same code // at higher level delta_angle = get_gyro(i) * get_delta_time(); return true; } return false; }
// update _gyro3_bias by comparing ins.get_gyro(2) with get_gyro void AP_AHRS_NavEKF::update_gyro3_bias(float dt) { static const float gyro3BiasFiltTC = 20.0f; if (!_ins.get_gyro_health(2)) { return; } // compute alpha float alpha = constrain_float(dt / (dt+gyro3BiasFiltTC),0.0f,1.0f); Vector3f gyro3_bias_unfiltered = _ins.get_gyro(2) - get_gyro(); _gyro3_bias += (gyro3_bias_unfiltered-_gyro3_bias)*alpha; }
/*************************************************************** Return the current state of all sensors ***************************************************************/ void return_all_sensors() { //printf(" in return_all_sensors() "); start_checksum(); push_char(0x50); push_char(0xAF); push_char(0x80); // fake data for now push_byte(get_sonar(1)); push_byte(get_sonar(2)); push_byte(get_sonar(3)); push_byte(get_line_follower(1)); push_byte(get_line_follower(2)); push_byte(get_line_follower(3)); push_byte(get_gyro()); push_byte(get_bumper(1)); push_byte(get_bumper(2)); send_checksum(); // Now 3 + 3 + 3 + 3 + 1 = 13 }
/* ===================================================================*/ void ReadData(void) { short Ax,Ay,Az,Mx,My,Mz,Gx,Gy,Gz; uint32 i; //extern TSENSORPtr tSENSORPtr; if(tSENSORPtr->sensorStatus.isDataReady == TRUE) { get_mag(&Mx,&My,&Mz); get_gyro(&Gx,&Gy,&Gz); get_acc(&Ax,&Ay,&Az); tSENSORPtr->sensorData.rawData[0]= Mx;//磁传感器变量; tSENSORPtr->sensorData.rawData[1]= My; tSENSORPtr->sensorData.rawData[2]= Mz; tSENSORPtr->sensorData.rawData[3]= Gx; tSENSORPtr->sensorData.rawData[4]= Gy; tSENSORPtr->sensorData.rawData[5]= Gz; tSENSORPtr->sensorData.rawData[6]= Ax; tSENSORPtr->sensorData.rawData[7]= Ay; tSENSORPtr->sensorData.rawData[8]= Az; tSENSORPtr->sensorStatus.transmitionContent = eData; tSENSORPtr->sensorStatus.isDataReady = FALSE; tSENSORPtr->sensorStatus.sensorDataStatus = eReceived; //printf("Mx=%d,My=%d,Mz=%d\n",Mx,My,Mz); //test the sample fre of read_data PTD0_50HZ_ClrVal(NULL); for(i=0;i<100;i++); PTD0_50HZ_SetVal(NULL); } }
void AP_InertialSensor::_init_gyro() { Vector3f last_average, best_avg; Vector3f ins_gyro; float best_diff = 0; // cold start hal.scheduler->delay(100); hal.console->printf_P(PSTR("Init Gyro")); // flash leds to tell user to keep the IMU still AP_Notify::flags.initialising = true; // remove existing gyro offsets _gyro_offset = Vector3f(0,0,0); for(int8_t c = 0; c < 25; c++) { hal.scheduler->delay(20); update(); ins_gyro = get_gyro(); } // the strategy is to average 200 points over 1 second, then do it // again and see if the 2nd average is within a small margin of // the first last_average.zero(); // we try to get a good calibration estimate for up to 10 seconds // if the gyros are stable, we should get it in 2 seconds for (int16_t j = 0; j <= 10; j++) { Vector3f gyro_sum, gyro_avg, gyro_diff; float diff_norm; uint8_t i; hal.console->printf_P(PSTR("*")); gyro_sum.zero(); for (i=0; i<200; i++) { update(); ins_gyro = get_gyro(); gyro_sum += ins_gyro; hal.scheduler->delay(5); } gyro_avg = gyro_sum / i; gyro_diff = last_average - gyro_avg; diff_norm = gyro_diff.length(); if (j == 0) { best_diff = diff_norm; best_avg = gyro_avg; } else if (gyro_diff.length() < ToRad(0.04)) { // we want the average to be within 0.1 bit, which is 0.04 degrees/s last_average = (gyro_avg * 0.5) + (last_average * 0.5); _gyro_offset = last_average; // stop flashing leds AP_Notify::flags.initialising = false; // all done return; } else if (diff_norm < best_diff) { best_diff = diff_norm; best_avg = (gyro_avg * 0.5) + (last_average * 0.5); } last_average = gyro_avg; } // stop flashing leds AP_Notify::flags.initialising = false; // we've kept the user waiting long enough - use the best pair we // found so far hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff)); _gyro_offset = best_avg; }
void AP_InertialSensor::_init_gyro() { uint8_t num_gyros = MIN(get_gyro_count(), INS_MAX_INSTANCES); Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES]; Vector3f new_gyro_offset[INS_MAX_INSTANCES]; float best_diff[INS_MAX_INSTANCES]; bool converged[INS_MAX_INSTANCES]; // exit immediately if calibration is already in progress if (_calibrating) { return; } // record we are calibrating _calibrating = true; // flash leds to tell user to keep the IMU still AP_Notify::flags.initialising = true; // cold start hal.console->print("Init Gyro"); /* we do the gyro calibration with no board rotation. This avoids having to rotate readings during the calibration */ enum Rotation saved_orientation = _board_orientation; _board_orientation = ROTATION_NONE; // remove existing gyro offsets for (uint8_t k=0; k<num_gyros; k++) { _gyro_offset[k].set(Vector3f()); new_gyro_offset[k].zero(); best_diff[k] = 0; last_average[k].zero(); converged[k] = false; } for(int8_t c = 0; c < 5; c++) { hal.scheduler->delay(5); update(); } // the strategy is to average 50 points over 0.5 seconds, then do it // again and see if the 2nd average is within a small margin of // the first uint8_t num_converged = 0; // we try to get a good calibration estimate for up to 30 seconds // if the gyros are stable, we should get it in 1 second for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) { Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES]; Vector3f accel_start; float diff_norm[INS_MAX_INSTANCES]; uint8_t i; memset(diff_norm, 0, sizeof(diff_norm)); hal.console->print("*"); for (uint8_t k=0; k<num_gyros; k++) { gyro_sum[k].zero(); } accel_start = get_accel(0); for (i=0; i<50; i++) { update(); for (uint8_t k=0; k<num_gyros; k++) { gyro_sum[k] += get_gyro(k); } hal.scheduler->delay(5); } Vector3f accel_diff = get_accel(0) - accel_start; if (accel_diff.length() > 0.2f) { // the accelerometers changed during the gyro sum. Skip // this sample. This copes with doing gyro cal on a // steadily moving platform. The value 0.2 corresponds // with around 5 degrees/second of rotation. continue; } for (uint8_t k=0; k<num_gyros; k++) { gyro_avg[k] = gyro_sum[k] / i; gyro_diff[k] = last_average[k] - gyro_avg[k]; diff_norm[k] = gyro_diff[k].length(); warnx("gyro[%d]: avg=%5.5f last_avg=%5.5f diff=%5.5f diff_norm=%5.5f\n", k, gyro_avg[k].length(), last_average[k].length(), gyro_diff[k].length(), diff_norm[k]); } for (uint8_t k=0; k<num_gyros; k++) { if (j == 0) { best_diff[k] = diff_norm[k]; best_avg[k] = gyro_avg[k]; } else if (gyro_diff[k].length() < ToRad(0.1f)) { // we want the average to be within 0.1 bit, which is 0.04 degrees/s last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f); if (!converged[k] || last_average[k].length() < new_gyro_offset[k].length()) { new_gyro_offset[k] = last_average[k]; } if (!converged[k]) { converged[k] = true; num_converged++; } } else if (diff_norm[k] < best_diff[k]) { best_diff[k] = diff_norm[k]; best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f); } last_average[k] = gyro_avg[k]; } } // we've kept the user waiting long enough - use the best pair we // found so far hal.console->println(); for (uint8_t k=0; k<num_gyros; k++) { if (!converged[k]) { hal.console->printf("gyro[%u] did not converge: diff=%f dps\n", (unsigned)k, (double)ToDeg(best_diff[k])); _gyro_offset[k] = best_avg[k]; // flag calibration as failed for this gyro _gyro_cal_ok[k] = false; } else { _gyro_cal_ok[k] = true; _gyro_offset[k] = new_gyro_offset[k]; } } // restore orientation _board_orientation = saved_orientation; // record calibration complete _calibrating = false; // stop flashing leds AP_Notify::flags.initialising = false; }
void AP_InertialSensor::_init_gyro() { uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES); Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES]; float best_diff[INS_MAX_INSTANCES]; bool converged[INS_MAX_INSTANCES]; // cold start hal.console->print_P(PSTR("Init Gyro")); // flash leds to tell user to keep the IMU still AP_Notify::flags.initialising = true; // remove existing gyro offsets for (uint8_t k=0; k<num_gyros; k++) { _gyro_offset[k] = Vector3f(0,0,0); best_diff[k] = 0; last_average[k].zero(); converged[k] = false; _gyro_cal_ok[k] = true; // default calibration ok flag to true } for(int8_t c = 0; c < 5; c++) { hal.scheduler->delay(5); update(); } // the strategy is to average 50 points over 0.5 seconds, then do it // again and see if the 2nd average is within a small margin of // the first uint8_t num_converged = 0; // we try to get a good calibration estimate for up to 30 seconds // if the gyros are stable, we should get it in 1 second for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) { Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES]; float diff_norm[INS_MAX_INSTANCES]; uint8_t i; memset(diff_norm, 0, sizeof(diff_norm)); hal.console->print_P(PSTR("*")); for (uint8_t k=0; k<num_gyros; k++) { gyro_sum[k].zero(); } for (i=0; i<50; i++) { update(); for (uint8_t k=0; k<num_gyros; k++) { gyro_sum[k] += get_gyro(k); } hal.scheduler->delay(5); } for (uint8_t k=0; k<num_gyros; k++) { gyro_avg[k] = gyro_sum[k] / i; gyro_diff[k] = last_average[k] - gyro_avg[k]; diff_norm[k] = gyro_diff[k].length(); } for (uint8_t k=0; k<num_gyros; k++) { if (converged[k]) continue; if (j == 0) { best_diff[k] = diff_norm[k]; best_avg[k] = gyro_avg[k]; } else if (gyro_diff[k].length() < ToRad(0.1f)) { // we want the average to be within 0.1 bit, which is 0.04 degrees/s last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f); _gyro_offset[k] = last_average[k]; converged[k] = true; num_converged++; } else if (diff_norm[k] < best_diff[k]) { best_diff[k] = diff_norm[k]; best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f); } last_average[k] = gyro_avg[k]; } } // stop flashing leds AP_Notify::flags.initialising = false; if (num_converged == num_gyros) { // all OK return; } // we've kept the user waiting long enough - use the best pair we // found so far hal.console->println(); for (uint8_t k=0; k<num_gyros; k++) { if (!converged[k]) { hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"), (unsigned)k, ToDeg(best_diff[k])); _gyro_offset[k] = best_avg[k]; // flag calibration as failed for this gyro _gyro_cal_ok[k] = false; } } }
void AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) { Vector3f last_average, best_avg; Vector3f ins_gyro; float best_diff = 0; // cold start delay_cb(100); if (_serial) _serial->printf_P(PSTR("Init Gyro")); // remove existing gyro offsets _gyro_offset = Vector3f(0,0,0); for(int16_t c = 0; c < 25; c++) { // Mostly we are just flashing the LED's here // to tell the user to keep the IMU still FLASH_LEDS(true); delay_cb(20); read(); update(); ins_gyro = get_gyro(); FLASH_LEDS(false); delay_cb(20); } // the strategy is to average 200 points over 1 second, then do it // again and see if the 2nd average is within a small margin of // the first last_average.zero(); // we try to get a good calibration estimate for up to 10 seconds // if the gyros are stable, we should get it in 2 seconds int num_attempts = 10; float target_average = 0.04; #ifdef INS_VRIMUFULL num_attempts = 5; target_average = 0.1 #endif for (int16_t j = 0; j <= num_attempts; j++) { Vector3f gyro_sum, gyro_avg, gyro_diff; float diff_norm; uint8_t i; if (_serial) _serial->printf_P(PSTR("*")); gyro_sum.zero(); for (i=0; i<200; i++) { //for (i=0; i<10; i++) { for(int z = 0; z < 10; z++){ read(); if (_serial) _serial->printf_P(PSTR(".")); } update(); ins_gyro = get_gyro(); // if (_serial) { // _serial->println(); // _serial->print(ins_gyro.x, 5); _serial->print(" "); // _serial->print(ins_gyro.y, 5); _serial->print(" "); // _serial->print(ins_gyro.z, 5); _serial->print(" "); // // } gyro_sum += ins_gyro; if (i % 40 == 20) { FLASH_LEDS(true); } else if (i % 40 == 0) { FLASH_LEDS(false); } delay_cb(5); } gyro_avg = gyro_sum / i; gyro_diff = last_average - gyro_avg; diff_norm = gyro_diff.length(); if (j == 0) { best_diff = diff_norm; best_avg = gyro_avg; } else if (gyro_diff.length() < ToRad(target_average)) { // we want the average to be within 0.1 bit, which is 0.04 degrees/s last_average = (gyro_avg * 0.5) + (last_average * 0.5); _gyro_offset = last_average; // all done if (_serial) { //_serial->printf_P(PSTR("\ngyro converged: diff=%f dps\n"), ToDeg(diff_norm)); _serial->printf_P(PSTR("\ngyro converged: diff=")); _serial->print(ToDeg(diff_norm), 5); _serial->printf_P(PSTR(" dps\n")); } return; } else if (diff_norm < best_diff) { best_diff = diff_norm; best_avg = (gyro_avg * 0.5) + (last_average * 0.5); } last_average = gyro_avg; } // we've kept the user waiting long enough - use the best pair we // found so far if (_serial) _serial->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff)); _gyro_offset = best_avg; }
// get roll rate in earth frame, in radians/s float AP_AHRS::get_roll_rate_earth(void) { Vector3f omega = get_gyro(); return omega.x + tanf(pitch)*(omega.y*sinf(roll) + omega.z*cosf(roll)); }
// get pitch rate in earth frame, in radians/s float AP_AHRS::get_pitch_rate_earth(void) { Vector3f omega = get_gyro(); return cosf(roll) * omega.y - sinf(roll) * omega.z; }
int calc_senc() { int i; double gyro=0.0; double max=0.0; double now_dis=0.0; double now_dis2=0.0; get_senc(); for(i=1; i<11; i++){ cari_SEN[i] = 100.0*(SEN[i]-SEN_min[i])/SEN_minmax[i]; dis_SEN[i] = (SEN[i]-SEN_min[i])/SEN_minmax[i]; sum_SEN[i] += cari_SEN[i]; sum_dis_SEN[i] += dis_SEN[i]; if(MAX_SEN[i] < cari_SEN[i]) MAX_SEN[i] = cari_SEN[i]; if(MIN_SEN[i] > cari_SEN[i]) MIN_SEN[i] = cari_SEN[i]; if(MAX_dis_SEN[i] < dis_SEN[i]) MAX_dis_SEN[i] = dis_SEN[i]; if(MIN_dis_SEN[i] > dis_SEN[i]) MIN_dis_SEN[i] = dis_SEN[i]; } ave_count++; if(ave_count==5){ sub_ERR=0.0; for(i=1; i<11; i++){ ave_SEN[i]=(sum_dis_SEN[i]-MAX_dis_SEN[i]-MIN_dis_SEN[i])/3.0; sum_dis_SEN[i]=0.0; MAX_dis_SEN[i]=0.0; MIN_dis_SEN[i]=0.0; ave_dis_SEN[i]=(sum_SEN[i]-MAX_SEN[i]-MIN_SEN[i])/3.0; sum_SEN[i]=0.0; MAX_SEN[i]=0.0; MIN_SEN[i]=0.0; } ave_count=0; } if(LED_test==1){ if(ave_SEN[corner] > 30.0) LED_manager(1); if(ave_SEN[corner] <= 30.0 && ave_SEN[corner] > 25.0) LED_manager(2); if(ave_SEN[corner] <= 25.0 && ave_SEN[corner] > 20.0) LED_manager(3); if(ave_SEN[corner] <= 20.0) LED_manager(4); } if(start0==1){ gyro=get_gyro(); gyro2=gyro-zero_gyro; if(gyro_first==0){ //基準値取得 target_gyro=gyro2; MAX_gyro=gyro2; MIN_gyro=gyro2; gyro_first++; } sum_gyro += gyro2; if(MAX_gyro < gyro2) MAX_gyro = gyro2; if(MIN_gyro > gyro2) MIN_gyro = gyro2; gyro_count++; if(gyro_count==5){ ave_gyro=(sum_gyro-MAX_gyro-MIN_gyro)/3.0; GYRO_ERR=target_gyro-ave_gyro; sum_gyro=0.0; MIN_gyro=MAX_gyro; MAX_gyro=0.0; gyro_count=0; } max=ave_dis_SEN[3]; for(i=1; i<7; i++){ if(max < ave_dis_SEN[i]) max=ave_dis_SEN[i]; } #if 0 if((ave_dis_SEN[2] > thre_max) && (ave_dis_SEN[3] < thre_max2 && ave_dis_SEN[4] < thre_max2 && ave_dis_SEN[5] < thre_max2 && ave_dis_SEN[6] < thre_max2)){ max_frag=1; } if(ave_dis_SEN[3] > thre_max || ave_dis_SEN[4] > thre_max || ave_dis_SEN[5] > thre_max || ave_dis_SEN[6] > thre_max) max_frag=0; if((ave_dis_SEN[1] < thre_max2 && ave_dis_SEN[2] < thre_max2 && ave_dis_SEN[3] < thre_max2 && ave_dis_SEN[4] < thre_max2) && (ave_dis_SEN[5] > thre_max)){ min_frag=1; } if(ave_dis_SEN[1] > thre_max || ave_dis_SEN[2] > thre_max || ave_dis_SEN[3] > thre_max || ave_dis_SEN[4] > thre_max) min_frag=0; #endif PRE_ERR=ERR; if((max==ave_dis_SEN[3] || max==ave_dis_SEN[4]) || ((ave_dis_SEN[1] < thre_max) && (ave_dis_SEN[2] < thre_max) && (ave_dis_SEN[5] < thre_max) && (ave_dis_SEN[6] < thre_max))){//○○● ●○○ now_dis=((senc_dis3*ave_dis_SEN[3])+(senc_dis4*ave_dis_SEN[4]))/(ave_dis_SEN[3]+ave_dis_SEN[4]); ERR=now_dis; } else if(((max_frag==0 && min_frag==0) && (max==ave_dis_SEN[2]) && (ave_dis_SEN[2] > thre_max) && goal_frag2==1)){//○●○ ○○○ now_dis=((senc_dis1*ave_dis_SEN[1])+(senc_dis2*ave_dis_SEN[2]))/(ave_dis_SEN[1]+ave_dis_SEN[2]); now_dis2=((senc_dis2*ave_dis_SEN[2])+(senc_dis3*ave_dis_SEN[3]))/(ave_dis_SEN[2]+ave_dis_SEN[3]); if(now_dis < now_dis2) ERR=now_dis2; else if(now_dis > now_dis2) ERR=now_dis; } else if(((max_frag==0 && min_frag==0) && (max==ave_dis_SEN[5]) && (ave_dis_SEN[5] > thre_max) && goal_frag2==1)){//○○○ ○●○ now_dis=((senc_dis4*ave_dis_SEN[4])+(senc_dis5*ave_dis_SEN[5]))/(ave_dis_SEN[4]+ave_dis_SEN[5]); now_dis2=((senc_dis5*ave_dis_SEN[5])+(senc_dis6*ave_dis_SEN[6]))/(ave_dis_SEN[5]+ave_dis_SEN[6]); if(now_dis < now_dis2) ERR=now_dis2; else if(now_dis > now_dis2) ERR=now_dis; } else if(((max_frag==0 && min_frag==0) && (max==ave_dis_SEN[1]) && (ave_dis_SEN[1] > thre_max) && goal_frag2==1)){//○○○ ○●○ now_dis=((senc_dis1*ave_dis_SEN[1])+(senc_dis2*ave_dis_SEN[2]))/(ave_dis_SEN[1]+ave_dis_SEN[2]); ERR=now_dis; } else if(((max_frag==0 && min_frag==0) && (max==ave_dis_SEN[6]) && (ave_dis_SEN[6] > thre_max) && goal_frag2==1)){//○○○ ○●○ now_dis=((senc_dis6*ave_dis_SEN[6])+(senc_dis5*ave_dis_SEN[5]))/(ave_dis_SEN[5]+ave_dis_SEN[6]); ERR=now_dis; } #if 0 if((max==ave_SEN[3] || max==ave_SEN[4]) || ((ave_SEN[1] < thre_max) && (ave_SEN[2] < thre_max) && (ave_SEN[5] < thre_max) && (ave_SEN[6] < thre_max))){ ERR=ave_SEN[3]-ave_SEN[4]; //○○● ●○○ sub_ERR=0.0; K_P=first_K_P; } else if(((max_frag==0 && min_frag==0) && (max==ave_SEN[2]) && (ave_SEN[2] > thre_max) && goal_frag2==1)){ dis_rate=ave_SEN[2]/ave_SEN_max[2]; now_dis=dis_rate*senc_dis2; ERR=now_dis; //○●○ ○○○ // sub_ERR=ave_SEN[9]-ave_SEN[10]; K_P=first_K_P*2.20; } else if(((max_frag==0 && min_frag==0) && (max==ave_SEN[5]) && (ave_SEN[5] > thre_max) && goal_frag2==1)){ ERR=(ave_SEN[3]-ave_SEN[5]); //○○● ○●○ sub_ERR=ave_SEN[9]-ave_SEN[10]; K_P=first_K_P*2.20; } #endif #if 0 else if((max_frag==0 && min_frag==0) && max==ave_SEN[1] && (ave_SEN[1] > thre_max) && goal_frag2==1){ ERR=(ave_SEN[1]-ave_SEN[4]); //●○○ ●○○ sub_ERR=ave_SEN[9]-ave_SEN[10]; K_P=first_K_P*2.5; } else if((max_frag==0 && min_frag==0) && max==ave_SEN[6] && (ave_SEN[6] > thre_max) && goal_frag2==1){ ERR=(ave_SEN[3]-ave_SEN[6]); //○○● ○○● sub_ERR=ave_SEN[9]-ave_SEN[10]; K_P=first_K_P*2.50; } #endif #if 0 if((goal_frag1==1 && max_frag==1 && (ave_SEN[2] < thre_max2 && ave_SEN[3] < thre_max2 && ave_SEN[4] < thre_max2 && ave_SEN[5] < thre_max2 && ave_SEN[6] < thre_max2))){ ERR=max_ERR; } if((goal_frag1==1 && min_frag==1 && (ave_SEN[1] < thre_max2 && ave_SEN[2] < thre_max2 && ave_SEN[3] < thre_max2 && ave_SEN[4] < thre_max2 && ave_SEN[5] < thre_max2))){ ERR=min_ERR; } #endif SUM_ERR+=ERR; } return 0; }
void trace() //一週目 { short start_frag=0; short trace_roop_frag=0; short i=0; short permit_goal=0; circl_count=1; LED1=ON; TIMER_WAIT(1000); LED1=OFF; ch_para(); //ch_para2(0); select_P(); // select_sub(); select_I(); // select_D(); // select_G(); select_goal(); // select_corner(); select_speed(); while(trace_roop_frag==0){ init_thre(); calc_senc(); if(cari_SEN[1] > 20.0){ if(Dispermit_goal==1){ start_frag=1; LED2=ON; TIMER_WAIT(500); } else{ LED2=ON; permit_goal=1; } } if(cari_SEN[6] > 20.0 && permit_goal==1){ start_frag=1; LED3=ON; TIMER_WAIT(500); } else if(cari_SEN[6] > 20.0 && permit_goal==0){ //ゴールさせない LED3=ON; Dispermit_goal=1; } if(start_frag==1){ countdown(); TIMER_WAIT(1000); for(i=0; i<201; i++){ ENC_dis[i]=0.0; ENC_check[i]=0; RL_check[i]=3; } for(i=0; i<5; i++) SC_box[i]=ST; zero_gyro=get_gyro(); ENC_R=15000; ENC_sub_R=0; ENC_L=15000; ENC_sub_L=0; EncTimeCount=0; ENC_count=0; dis_count=0; mot_STB(START_A); start0=1; mot_frag=1; LED1=OFF; while(stop_frag==0){ if(PORTE.PORT.BIT.B0 == 0){ stop_frag=1; TIMER_WAIT(500); } } stop_manager(); trace_roop_frag=1; } else{} } }
void trace2() //二週目 { short start_frag=0; short trace_roop_frag=0; short permit_goal=0; circl_count=2; LED2=ON; TIMER_WAIT(1000); LED2=OFF; // ch_para2(0); ch_para(); select_P(); select_I(); // select_D(); // select_G(); select_goal(); // select_corner(); select_speed(); while(trace_roop_frag==0){ init_thre(); calc_senc(); if(cari_SEN[1] > 20.0){ if(Dispermit_goal==1){ start_frag=1; LED2=ON; TIMER_WAIT(500); } else{ LED2=ON; permit_goal=1; } } if(cari_SEN[6] > 20.0 && permit_goal==1){ start_frag=1; LED3=ON; TIMER_WAIT(500); } else if(cari_SEN[6] > 20.0 && permit_goal==0){ //ゴールさせない LED3=ON; Dispermit_goal=1; } if(start_frag==1){ countdown(); TIMER_WAIT(1000); zero_gyro=get_gyro(); ENC_R=15000; ENC_sub_R=0; ENC_L=15000; ENC_sub_L=0; EncTimeCount=0; ENC_count=0; dis_count=0; mot_STB(START_A); start0=1; mot_frag=1; while(stop_frag==0){} stop_manager(); trace_roop_frag=1; } else{} } }
void trace3() //3週目 { int start_frag[3]; int trace_roop_frag=0; int roop_frag=0; short para_frag=0; circl_count=3; LED3=ON; TIMER_WAIT(1000); LED3=OFF; while(trace_roop_frag==0){ if(PORTE.PORT.BIT.B0 == 0){ LED1=OFF; TIMER_WAIT(500); while(roop_frag==0){ if(para_frag==0){ select_P(); ch_para(); para_frag=1; } calc_senc(); if(cari_SEN[1] > 40){ start_frag[1]=1; LED2=ON; } if(start_frag[1]==1){ if(cari_SEN[6] > 40){ start_frag[2]=1; LED3=ON; TIMER_WAIT(500); } } if(start_frag[2]==1){ countdown(); TIMER_WAIT(1000); mot_STB(START_A); zero_gyro=get_gyro(); ENC_R=0; ENC_sub_R=0; ENC_L=0; ENC_sub_L=0; start0=1; while(stop_frag==0){} stop_manager(); trace_roop_frag=1; roop_frag=1; } else{} } } else{} } }