/*
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{}
}
}
