Commit 003061d0 authored by ctrenkov's avatar ctrenkov
Browse files

mixer solver works

parent 6d89a09f
......@@ -4,13 +4,13 @@
<table id="motor-config-table" class="mixer-table">
<thead>
<tr>
<th style="width: 75px">IsPusher</th>
<th style="width: 75px">CW</th>
<th>PosX</th>
<th>PosY</th>
<th>PosZ</th>
<th>RotX</th>
<th>RotY</th>
<th>RotZ</th>
<th>DirX</th>
<th>DirY</th>
<th>DirZ</th>
<th class="delete">Remove</th>
</tr>
</thead>
......@@ -19,4 +19,6 @@
</tbody>
</table>
<span class="btn default_btn narrow green" id="add-motor"><a href="#" >Add motor</a></span>
<span class="btn default_btn narrow green" id="launch-solver"><a href="#" >Solve!</a></span>
</div>
......@@ -12,13 +12,13 @@ function createRow(){
//IK this is terrible. But I will never pay for these crimes
row.innerHTML = `
<td><input type="checkbox"/></td>
<td><input type="number" step="0.1" min="-10" max="10" value="1"/></td>
<td><input type="number" class="mix-rule-throttle" step="0.01" min="-10" max="10" value=".5"/></td>
<td><input type="number" class="mix-rule-pitch" step="0.01" min="-10" max="10" value="0"/></td>
<td><input type="number" class="mix-rule-pitch" step="0.01" min="-10" max="10" value="0"/></td>
<td><input type="number" class="mix-rule-pitch" step="0.01" min="-10" max="10" value="0"/></td>
<td><input type="number" class="mix-rule-pitch" step="0.01" min="-10" max="10" value="0"/></td>
<td><input type="number" class="mix-rule-pitch" step="0.01" min="-10" max="10" value="0"/></td>
<td><input type="number" class="mix-rule-pitch" step=".1" min="-1" max="1" value="0"/></td>
<td><input type="number" class="mix-rule-pitch" step=".1" min="-1" max="1" value="1"/></td>
<td><input type="number" class="mix-rule-pitch" step=".1" min="-1" max="1" value="0"/></td>
<td><span class="btn default_btn narrow red"><a href="#" data-role="role-motor-delete" data-i18n="servoMixerDelete">Remove</a></span></td>
`;
......@@ -81,8 +81,21 @@ TABS.drone_canvas.initialize = function (callback) {
return isNaN(parseFloat(value)) ? value : parseFloat(value);
}
function to_rad(deg){
return 3.1415926 * deg / 180;
}
motor.position.set(get_value(1), get_value(2), get_value(3));
motor.rotation.set(get_value(4), get_value(5), get_value(6));
const quaternion = new THREE.Quaternion(); // create one and reuse it
const dir = new THREE.Vector3(get_value(4), get_value(5), get_value(6));
dir.normalize();
quaternion.setFromUnitVectors( new THREE.Vector3(0, 1, 0), dir );
const euler = new THREE.Euler()
euler.setFromQuaternion(quaternion);
motor.rotation.set(euler.x, euler.y, euler.z);
}
//Pop an event listener to each input for the row
......@@ -133,11 +146,84 @@ TABS.drone_canvas.initialize = function (callback) {
renderer.setSize(render_width, render_height)
}
function getMotorMatrix(){
console.log("MOTOR");
table = document.getElementById('motor-parameters');
const spin = [];
const motors = [];
for(row of table.children){
spin.push(parseFloat(row.children[0].children[0].value));
const pos = []
for(var i = 1; i < 4; i++){
pos.push(parseFloat(row.children[i].children[0].value))
}
const dir = []
for(var i = 4; i < 7; i++){
dir.push(parseFloat(row.children[i].children[0].value))
}
motors.push([pos, dir]);
}
return [spin, motors];
}
function add_force(system, motors, spin){
for(var i = 0; i < motors.length; i++){
//torque force
const motor_pos = motors[i][0];
const motor_dir = motors[i][1];
const c = math.cross(motor_pos, motor_dir);
for(var j = 0; j < 3; j++){
system[i][j] = c[j];
}
//lateral force
for(var j = 0; j < 3; j++){
system[i][j+3] = motors[i][1][j];
}
//yaw force
for(var j = 0; j < 3; j++){
system[i][j] += -spin[i] * motors[i][1][j];
}
}
}
function onAttemptSolve(){
const mat = math.matrix([[-1, 0], [0, -1]]);
console.log(math.inv(mat));
const [spin, motors] = getMotorMatrix();
const system = math.zeros([spin.length, 6]);
add_force(system, motors, spin);
const inv = math.inv(math.multiply(system, math.transpose(system)));
const t_inv = math.multiply(inv, system);
const mixer = math.zeros([spin.length, 6]);
for(var i = 0; i < 6; i++){
const vector = math.zeros([6, 1])
vector[i][0] = 1
solve = math.multiply(t_inv, vector)
for(var j = 0; j < spin.length; j++){
mixer[j][i] = solve[j][0];
}
}
console.log("MIXEr");
console.log(mixer);
console.log(math.round(math.multiply(mixer, math.transpose([0, 0, 1, 0, 1, 0])), 2));
}
onAttemptSolve();
//onAttemptSolve();
document.getElementById('launch-solver').addEventListener('click', onAttemptSolve);
});
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment