bool Camera::_get(const StringName& p_name,Variant &r_ret) const {
if (p_name=="projection") {
r_ret= mode;
} else if (p_name=="fov" || p_name=="fovy" || p_name=="fovx")
r_ret= fov;
else if (p_name=="size" || p_name=="sizex" || p_name=="sizey")
r_ret= size;
else if (p_name=="near")
r_ret= near;
else if (p_name=="far")
r_ret= far;
else if (p_name=="keep_aspect")
r_ret= int(keep_aspect);
else if (p_name=="current") {
if (is_inside_tree() && get_tree()->is_node_being_edited(this)) {
r_ret=current;
} else {
r_ret=is_current();
}
} else if (p_name=="visible_layers") {
r_ret=get_visible_layers();
} else if (p_name=="h_offset") {
r_ret=get_h_offset();
} else if (p_name=="v_offset") {
r_ret=get_v_offset();
} else if (p_name=="environment") {
r_ret=get_environment();
} else
return false;
return true;
}
void do_goahead(void)
{
double v_offset; //电压值偏离标准量v_offset，电压值输出量v_output
int v_maxone, v_output;
static int r1_confirm_count = 0;
if (HAS_REACHED_CROSS_LINE() || head_passed_cross) {
r1_confirm_count ++;
if (r1_confirm_count > 2) {
can_interrupt = 0; // 一到十字就锁住
head_passed_cross = 1;
control_call_callback(C_AHEAD, E_CROSS);
}
}
else {
r1_confirm_count = 0;
}
static int r3r4_confirm_count = 0;
if(IS_AT_CORNER() && head_passed_cross) {
r3r4_confirm_count ++;
if (r3r4_confirm_count > 2) {
can_interrupt = 1;
control_call_callback(C_AHEAD, E_ONCROSS);
head_passed_cross = 0;
}
}
else {
r3r4_confirm_count = 0;
}
if(goahead_distance_set > goahead_distance_count)
{
goahead_distance_count += pulsecount;
v_offset = get_v_offset();
v_maxone = V_MAXONE(V_MAX);
if(v_offset > 0)
{
v_output = v_maxone - (int) v_offset;
if(v_output < 0) v_output = 0;
right_set_v((int) v_output);
left_set_v((int) v_maxone);
}
else
{
v_output = v_maxone + (int) v_offset;
if(v_output < 0) v_output = 0;
right_set_v((int) v_maxone);
left_set_v((int) v_output);
}
}
else
{
// 完成，重置
goahead_distance_count = 0;
goahead_distance_set = 0;
routechoice = C_STOP;
can_interrupt = 1;
control_call_callback(C_AHEAD, E_FINISHED);
}
}
bool PathFollow2D::_get(const StringName& p_name,Variant &r_ret) const{
if (p_name==SceneStringNames::get_singleton()->offset) {
r_ret=get_offset();
} else if (p_name==SceneStringNames::get_singleton()->unit_offset) {
r_ret=get_unit_offset();
} else if (p_name==SceneStringNames::get_singleton()->rotate) {
r_ret=is_rotating();
} else if (p_name==SceneStringNames::get_singleton()->v_offset) {
r_ret=get_v_offset();
} else if (p_name==SceneStringNames::get_singleton()->h_offset) {
r_ret=get_h_offset();
} else if (String(p_name)=="cubic_interp") {
r_ret=cubic;
} else if (String(p_name)=="loop") {
r_ret=loop;
} else if (String(p_name)=="lookahead") {
r_ret=lookahead;
} else
return false;
return true;
}
void OrientedPathFollow::_update_transform() {
if (!path)
return;
Ref<Curve3D> c = path->get_curve();
if (!c.is_valid())
return;
int count = c->get_point_count();
if (count < 2)
return;
if (delta_offset == 0) {
return;
}
float offset = get_offset();
float bl = c->get_baked_length();
float bi = c->get_bake_interval();
float o = offset;
float o_next = offset + bi;
if (has_loop()) {
o = Math::fposmod(o, bl);
o_next = Math::fposmod(o_next, bl);
} else if (o_next >= bl) {
o = bl - bi;
o_next = bl;
}
bool cubic = get_cubic_interpolation();
Vector3 pos = c->interpolate_baked(o, cubic);
Vector3 forward = c->interpolate_baked(o_next, cubic) - pos;
if (forward.length_squared() < CMP_EPSILON2)
forward = Vector3(0, 0, 1);
else
forward.normalize();
Vector3 up = c->interpolate_baked_up_vector(o, true);
if (o_next < o) {
Vector3 up1 = c->interpolate_baked_up_vector(o_next, true);
Vector3 axis = up.cross(up1);
if (axis.length_squared() < CMP_EPSILON2)
axis = forward;
else
axis.normalize();
up.rotate(axis, up.angle_to(up1) * 0.5f);
}
Transform t = get_transform();
Vector3 scale = t.basis.get_scale();
Vector3 sideways = up.cross(forward).normalized();
up = forward.cross(sideways).normalized();
t.basis.set(sideways, up, forward);
t.basis.scale_local(scale);
t.origin = pos + sideways * get_h_offset() + up * get_v_offset();
set_transform(t);
}
void do_letswin(void) {
double v_offset; //电压值偏离标准量v_offset，电压值输出量v_output
int v_maxone, v_output;
static int letwin_back_time=0;
if(LETSWIN_BACK_TIME > letwin_back_time)
{
letwin_back_time++;
right_set_v(-3000);
left_set_v(-3000);
}
else
{
if(LETSWIN_GOHEAD_DISTANCE > letswin_distance_count)
{
__start_beep=2;
letswin_distance_count += 1;//pulsecount;
v_offset = get_v_offset();
v_maxone = 3000;
if(v_offset > 0)
{
v_output = v_maxone - (int) v_offset;
if(v_output < 0) v_output = 0;
right_set_v((int) v_output);
left_set_v((int) v_maxone);
}
else
{
v_output = v_maxone + (int) v_offset;
if(v_output < 0) v_output = 0;
right_set_v((int) v_maxone);
left_set_v((int) v_output);
}
}
else
{
static int __r3_confirm = 0;
static char have_past_black=0;
static unsigned int __letswin_shake_count=0;
if ( !r3[2]||(r4[1]||r4[2]||r4[0]))
{
__r3_confirm = 0;
}
else
{
__r3_confirm ++;
}
if (__r3_confirm > 2) {
have_past_black=1;
}
if(have_past_black)//&&!__r3_confirm
{
__letswin_shake_count++;
if(__letswin_shake_count/200%2)
{
right_set_v(3000);
left_set_v(-1000);
}
else
{
right_set_v(LETSWIN_V_R_SECOND_STEP);
left_set_v(LETSWIN_V_L_SECOND_STEP);
}
}
else {
right_set_v(LETSWIN_V_R_FIRST_STEP);
left_set_v(LETSWIN_V_L_FIRST_STEP);
}
static int __letswin_time_count = 0;
if (__letswin_time_count < 600) __letswin_time_count ++;
else
{
routechoice = C_STOP;
can_interrupt = 1;
letswin_distance_count = 0;
letwin_back_time=0;
}
}
}
}
void do_getcorner(void)
{
double v_offset = 0.0;
int v_maxone, v_output = 0;
static int r1_confirm_count = 0;
if(HAS_REACHED_CROSS_LINE() || head_passed_cross)
{
r1_confirm_count ++;
if (r1_confirm_count > 2) {
//getcorner_finish=1;
//robot_controlable_downgrade=0;
has_began_getcorner = 1;
head_passed_cross = 1;
can_interrupt = 0;
control_call_callback(C_GETCORNER, E_CROSS);
}
}
else {
r1_confirm_count = 0;
}
if(!has_began_getcorner)//前排检测到十字没有
{
//robot_controlable_downgrade=1;
//can_interrupt = 1; // 未到十字，可被打断
v_offset = get_v_offset();
v_maxone = V_MAXONE(V_GETCORNER_MAX);
if(v_offset > 0)
{
v_output = v_maxone - (int) v_offset;
if(v_output < 0) v_output = 0;
right_set_v((int) v_output);
left_set_v((int) v_maxone);
}
else
{
v_output = v_maxone + (int) v_offset;
if(v_output < 0) v_output = 0;
right_set_v((int) v_maxone);
left_set_v((int) v_output);
}
}
else//检测到十字
{
static int r3r4_confirm_count = 0;
if(IS_AT_CORNER()) {
r3r4_confirm_count ++;
if (r3r4_confirm_count > 2) {
atcorner = 1;
head_passed_cross = 0;
}
}
else {
r3r4_confirm_count = 0;
}
if(atcorner == 0)//是否停在正下方
{
//do_stop();//暂时用停止代替 等会换成反刹
if(pulsecount <= 4)//减速到编码器值为4 刹车结束//有反退的危险啊2的话
{
//v_offset = get_v_offset();
//v_maxone=2000;//以低速到达十字正下方
right_set_v(V_SLOW);
left_set_v(V_SLOW);
}
else
{
// 刹车
right_set_v(-V_BREAK);
left_set_v(-V_BREAK);
}
}
else
{
if(motor_time_wait < MOTOR_RESPONSE_WAIT)//等待电机反应时间 不然直接下一步有影响 没有彻底停下来
{
motor_time_wait ++;
right_set_v(0);
left_set_v(0);
}
else
{
atcorner = 0;
motor_time_wait = 0;
has_began_getcorner = 0;//重置状态
can_interrupt = 1;
routechoice = C_STOP;
control_call_callback(C_GETCORNER, E_FINISHED);
}
}
}
}
