var speed = 0 var angular_speed_target = 0 var linear_speed_target = 0 var linear_speed_error = 0 var ir_error = 0 var ir_error_old = 0 var ir_error_acc = 0 var angle_error = 0 var mode = STOP var angle = 0 var ir_target_adapt_0 = IR_TARGET_0 var ir_target_adapt_1 = IR_TARGET_1 var stable_counter = 0 var prox_accumulation[2] = [0,0] var angle_history[TAB_SIZE+1] var j=0 var i var stable var mean_acc = 0 var dump var variance var temporary_variance onevent prox if mode >= START then #static ir_error = (prox.ground.delta[0] - IR_TARGET_0)# + prox.ground.delta[1] - IR_TARGET_1) #adaptif #ir_error = (prox.ground.delta[0] - ir_target_adapt_0 + prox.ground.delta[1] - ir_target_adapt_1) if ir_error > 0 then ir_error = ir_error / COEFF_ADJUST end ir_error_acc += ir_error speed = (KP * ir_error + KD * (ir_error - ir_error_old) + KI * ir_error_acc)/COEFF_DIV if ir_error_acc > ARW then ir_error_acc = ARW elseif ir_error_acc < -ARW then ir_error_acc = -ARW end call math.atan2(angle,acc[1],acc[2]) if j<TAB_SIZE then angle_history[j] = angle j++ else angle_history[j] = angle j=0 end call math.stat(angle_history, dump, dump, mean_acc) temporary_variance = 0 for i in 0:TAB_SIZE do temporary_variance += abs(angle_history[i] - mean_acc) end variance = abs(temporary_variance / (TAB_SIZE+1)) emit variance variance if variance < THRESHOLD_STABILITY then if angle < ANGLE_MIN_ADMISSIBLE and speed < MAX_SPEED_ADAPTATOR then ir_target_adapt_0-- ir_target_adapt_1-- elseif angle > ANGLE_MAX_ADMISSIBLE and speed < MAX_SPEED_ADAPTATOR then ir_target_adapt_0++ ir_target_adapt_1++ end end ir_error_old = ir_error if speed > 500 then speed = 500 elseif speed < -500 then speed = -500 end emit angle_val angle emit proxIR prox.ground.delta emit speed_val speed emit error_speed [ir_error, speed] if mode == START_MOT then motor.left.target = speed motor.right.target = speed end else motor.left.target = 0 motor.right.target = 0 end onevent rc5 call math.fill(angle_history,0) i = 0 ir_target_adapt_0 = IR_TARGET_0 ir_target_adapt_1 = IR_TARGET_1 stable_counter = 0 prox_accumulation[0] = 0 prox_accumulation[1] = 0 if rc5.command == 12 then mode = STOP elseif rc5.command == 1 then mode = START elseif rc5.command == 2 then mode = START_MOT end