Open JamesNewton opened 6 years ago
Another way of doing this is to use a non-golden physical disk and then compensate for the errors after the fact. Given a way of measuring the overall actual accuracy, which James W has developed, we need a way of injecting that compensation in to the system. That could be via firmware, gateware, or by editing the himem.dat file.
var result
var J_num = 0
var J_angles = [0, 0, 0, 0, 0]
function example_5_init(){
let CMD = []
result = {}
J_angles = [-180, 0, 0, 0, 0]
CMD.push(Dexter.move_all_joints(correct_J_angles(J_angles)))
CMD.push(Dexter.empty_instruction_queue())
this.user_data.C_state_old = false
return CMD
}
function example_5_main(){
return Robot.loop(true, function(){
let CMD = []
let rs = this.robot.robot_status
let measured_angles = [
rs[Dexter.J1_MEASURED_ANGLE],
rs[Dexter.J2_MEASURED_ANGLE],
rs[Dexter.J3_MEASURED_ANGLE],
rs[Dexter.J4_MEASURED_ANGLE],
rs[Dexter.J5_MEASURED_ANGLE]
]
let J2 = measured_angles[J_num]
let J2_snapped = Math.round(J2/5)*5 + 5
let string = J2_snapped + " : " + Vector.round(J2, 3)
out(string, "blue", true)
if(Gamepad.is_key_down("CONTROL") && Gamepad.is_key_down("ALT")){
let step
if(Gamepad.is_key_down("SHIFT")){
step = 5
}else if(Gamepad.is_key_down("Z")){
step = 0.1
}else if(Gamepad.is_key_down("X")){
step = 0.01
}else{
step = 1
}
let delta_J_angles = [0, 0, 0, 0, 0]
if(Gamepad.is_key_down("LEFT")){
delta_J_angles[J_num] = step
}else if(Gamepad.is_key_down("RIGHT")){
delta_J_angles[J_num] = -step
}
//CMD.push(Dexter.move_all_joints_relative(delta_J_angles))
debugger
J_angles = Vector.add(J_angles, delta_J_angles)
CMD.push(Dexter.move_all_joints(correct_J_angles(J_angles)))
if(Gamepad.is_key_down("A")){
this.user_data.C_state_old = true
}else if(this.user_data.C_state_old){
speak(string)
out("Recorded: " + string)
result["" + J2_snapped] = J2
this.user_data.C_state_old = false
}
}else if(Gamepad.is_key_down("ALT") && Gamepad.is_key_down("C")){
return Robot.break()
}
CMD.push(Dexter.empty_instruction_queue())
return CMD
})
}
function example_5_final(){
let CMD = []
CMD.push(Dexter.move_all_joints([0, 0, 0, 0, 0]))
return CMD
}
new Job({
name: "Record_Data",
keep_history: false,
show_instructions: false,
inter_do_item_dur: 0,
do_list: [
example_5_init,
example_5_main,
example_5_final
]
})
new Brain({name: "b0"})
new Job({
name: "Save_Data",
robot: Brain.b0,
keep_history: false,
show_instructions: false,
inter_do_item_dur: 0,
do_list: [
function(){
let str = JSON.stringify(sort_and_reformat(result))
let fp = choose_save_file()
if(fp){
write_file(fp, str)
}
}
]
})
function correct_J_angles(J_angles){
let result = JSON.parse(JSON.stringify(J_angles))
let coeffs = [
[0.017772801117188035, -0.16017917223939698],
[1, 0],
[1, 0],
[1, 0],
[1, 0]
]
for(let i = 0; i < coeffs.length; i++){
result[i] -= coeffs[i][0] * result[i] + coeffs[i][1]
//J_angles[i] += (J_angles[i] - coeffs[i][1]) / coeffs[i][0]
}
return result
}
//correct_J_angles([0, 0, 0, 0, 0])
/*
result
var fp = choose_file()
var content = file_content(fp)
var result = JSON.parse(content)
//inspect(result)
for(let i = 0; i < result.ideal.length; i++){
if(9 <= i && i <= 58){
result.ideal[i] -= 5
}else if(59 <= i && i <= 66){
result.ideal[i] -= 10
}else if(67 <= i && i <= 67){
result.ideal[i] -= 5
}else if(68 <= i && i <= 68){
result.ideal[i] -= 10
}
}
var str = JSON.stringify(result)
let fp = choose_save_file()
if(fp){
write_file(fp, str)
}
var errors = Vector.subtract(result.actual, result.ideal)
var coeffs = Vector.poly_fit(result.ideal, errors)
debugger
my_sort(result)
result.sort(function(a, b){
debugger
return parseInt(Object.keys(a)) > parseInt(Object.keys(b))
})
pid_delta = 0.570
*/
function sort_and_reformat(obj){
let keys = Object.keys(obj)
let keys_as_nums = []
for(let i = 0; i < keys.length; i++){
keys_as_nums.push(parseInt(keys[i]))
}
let keys_as_nums_sorted = keys_as_nums.sort(function(a, b){
return a - b
})
let result = {
ideal: keys_as_nums_sorted,
actual: []
}
for(let i = 0; i < keys_as_nums_sorted.length; i++){
result.actual.push(obj["" + keys_as_nums_sorted[i]])
}
return result
}
//This is code to sense Keyboard button presses
//It's a patch to impliment a feature that has been introduced in later releases
Gamepad.is_key_down = function(keycode_or_keyname){
let down_keys = this.down_keys()
for(let obj of down_keys){
if(obj.keycode == keycode_or_keyname){
return true
}else if(obj.keyname == keycode_or_keyname){
return true
}
}
return false
}
Kamino cloned this issue to HaddingtonDynamics/OCADO
Golden code disk is oversized, and so very precise, and is used to calibrate actual disks during mfgr. This will improve the accuracy of the encoders.