Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- <!DOCTYPE aesl-source>
- <network>
- <!--list of global events-->
- <event size="0" name="start_calibration"/>
- <event size="0" name="follow_line"/>
- <event size="2" name="vars"/>
- <!--list of constants-->
- <constant value="2" name="CALIBRATION_STEPS"/>
- <constant value="35" name="MAX_REFLECTION_VARIATION_PERCENTAGE"/>
- <!--show keywords state-->
- <keywords flag="true"/>
- <!--node thymio-II-->
- <node nodeId="7198" name="thymio-II">var calibrating = -1
- var following_line = -1
- #var min_ambiant[2] = [32767, 32767]
- #var max_ambiant[2] = [-32768, -32768]
- var min_reflected[2] = [32767, 32767]
- var max_reflected[2] = [-32768, -32768]
- var white_threshold[2] = [-32768, -32768]
- #var black_threshold[2] = [32767, 32767]
- # [((last) time, motor.left, motor.right), (pre-last), ...]
- var calibration_values[CALIBRATION_STEPS * 3] = [0, 0, 0, 3000, 200, 200]
- #[0, 0, 0, 3000, 100, 100, 2500, 100, -100, 3000, 100, 100, 2500, 100, -100, 3000, 100, 100, 2500, 100, -100, 3000, 100, 100]
- var var1 = -1
- var var2 = -1
- var var3 = -1
- var var4 = -1
- var lastTurned = 0
- var emergency = 0
- timer.period[0] = 0
- timer.period[1] = 0
- sub start_calibration
- #min_ambiant = [32767, 32767]
- #max_ambiant = [-32768, -32768]
- min_reflected = [32767, 32767]
- max_reflected = [-32768, -32768]
- calibrating = CALIBRATION_STEPS
- timer.period[0] = 100
- return
- sub update_calibration
- # min_ambiant
- #call math.min(min_ambiant[0], min_ambiant[0], prox.ground.ambiant[0])
- #call math.min(min_ambiant[1], min_ambiant[1], prox.ground.ambiant[1])
- # max_ambiant
- #call math.max(max_ambiant[0], max_ambiant[0], prox.ground.ambiant[0])
- #call math.max(max_ambiant[1], max_ambiant[1], prox.ground.ambiant[1])
- # min_reflected
- call math.min(min_reflected[0], min_reflected[0], prox.ground.reflected[0])
- call math.min(min_reflected[1], min_reflected[1], prox.ground.reflected[1])
- # max_reflected
- call math.max(max_reflected[0], max_reflected[0], prox.ground.reflected[0])
- call math.max(max_reflected[1], max_reflected[1], prox.ground.reflected[1])
- return
- sub follow_line
- following_line = 1
- return
- onevent prox
- # collision detection
- if (motor.left.target != -motor.right.target) and (((motor.left.target > 20 or motor.right.target > 20) and (prox.horizontal[0] > 3000 or prox.horizontal[1] > 3000 or prox.horizontal[2] > 3000 or prox.horizontal[3] > 3000 or prox.horizontal[4] > 3000)) or ((motor.left.target < -20 or motor.right.target < -20) and (prox.horizontal[5] > 3000 or prox.horizontal[6] > 3000))) then
- motor.left.target = 0
- motor.right.target = 0
- following_line = 0
- return
- end
- if calibrating > 0 then
- callsub update_calibration
- elseif following_line > 0 then
- var1 = prox.ground.reflected[0]
- var2 = prox.ground.reflected[1]
- lastTurned = 0
- if var1 < white_threshold[0] then
- var1 = 500
- lastTurned = lastTurned - 1
- else
- var1 = 0
- end
- if var2 < white_threshold[1] then
- var2 = 500
- else
- var2 = 0
- lastTurned = lastTurned + 1
- end
- motor.left.target = var2
- motor.right.target = var1
- if var1 == 0 and var2 == 0 then
- if emergency == 0 then
- if lastTurned != 0 then
- emergency = lastTurned
- timer.period[1] = 1000
- end
- elseif emergency == 2 then
- # lost the line :(
- emergency = 0
- following_line = 0
- else
- # emergency turn
- motor.left.target = 200 * emergency
- motor.right.target = -200 * emergency
- end
- else
- emergency = 0
- end
- end
- return
- onevent timer0
- if calibrating > 0 then
- if calibrating == 1 then
- var1 = (max_reflected[0] - min_reflected[0]) / 10 * MAX_REFLECTION_VARIATION_PERCENTAGE / 10
- white_threshold[0] = max_reflected[0] - var1
- var1 = (max_reflected[1] - min_reflected[1]) / 10 * MAX_REFLECTION_VARIATION_PERCENTAGE / 10
- white_threshold[1] = max_reflected[1] - var1
- timer.period[0] = 0
- end
- calibrating--
- timer.period[0] = calibration_values[calibrating*3]
- motor.left.target = calibration_values[(calibrating*3)+1]
- motor.right.target = calibration_values[(calibrating*3)+2]
- end
- return
- onevent timer1
- if emergency != 0 then
- emergency = 2
- timer.period[1] = 0
- end
- return
- onevent start_calibration
- callsub start_calibration
- return
- onevent follow_line
- callsub follow_line
- return</node>
- </network>
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement