Skip to content

Commit

Permalink
Switched more stuff to new setup
Browse files Browse the repository at this point in the history
  • Loading branch information
@misprit7
misprit7 committed Feb 16, 2024
1 parent bc5bf84 commit d0bea1f
Show file tree
Hide file tree
Showing 3 changed files with 97 additions and 91 deletions.
21 changes: 12 additions & 9 deletions frontend/app.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,9 @@ loader.load('assets/table.glb', function(gltf) {
* Set up websocket
******************************************************************************/

const ws = new WebSocket('ws://localhost:9001/position');
// const ws = new WebSocket('ws://localhost:9001/position');
// const ws = new WebSocket('ws://75.157.213.247:9001/position');
const ws = new WebSocket('ws://192.168.1.77:9001/position');

// Should probably be a callback when ws connects
setTimeout(function() {
Expand All @@ -108,11 +110,11 @@ setTimeout(function() {
const bluerod = blue_rods.children[i];
bluerod.position.z = bluerod.offset + (packet['bluepos'][i]-1/2)*limits[i];

red_rods.children[i].rotation.y = (packet['redrot'][i]);
blue_rods.children[i].rotation.y = (packet['bluerot'][i]);
red_rods.children[i].rotation.y = (packet['redrot'][i] / 360 * (2*Math.PI));
blue_rods.children[i].rotation.y = (packet['bluerot'][i] / 360 * (2*Math.PI));
}
ball.position.x = (packet['ballpos'][1]-0.5) * table_height;
ball.position.z = (packet['ballpos'][0]-0.5) * table_width;
ball.position.x = (packet['ballpos'][1]) * table_height;
ball.position.z = (packet['ballpos'][0] - 0.5) * table_width;
}
}
}, 300);
Expand Down Expand Up @@ -227,8 +229,8 @@ function animate() {

outlinePass.selectedObjects = [rod];

const lin_speed = 0.20;
const rot_speed = 0.2;
const lin_speed = 0.10;
const rot_speed = 0.10;

let dz = 0, drot = 0;
if(gamepads.length > 0){
Expand All @@ -237,6 +239,7 @@ function animate() {
const joystickLeftY = gamepad.axes[1];
const joystickRightX = gamepad.axes[2];
const joystickRightY = gamepad.axes[3];
const leftTrigger = gamepad.buttons[7].pressed;

// console.log(gamepad.axes[5]);
if(gamepad.axes[5] != dpad_y_last){
Expand All @@ -246,8 +249,8 @@ function animate() {
}
}

dz = lin_speed * joystickLeftX;
drot = rot_speed * joystickRightY;
dz = lin_speed * joystickLeftX / (leftTrigger ? 4 : 1);
drot = rot_speed * joystickRightY / (leftTrigger ? 4 : 1);
} else {
dz = (left_pressed ?- lin_speed : 0) + (right_pressed ? lin_speed : 0);
drot = (rot_down_pressed ?- rot_speed : 0) + (rot_up_pressed ? rot_speed : 0);
Expand Down
157 changes: 80 additions & 77 deletions software/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -479,9 +479,6 @@ int main(int argc, char** argv){
**************************************************************************/

// Do this on the main thread just to make sure that everything is initialized
int init_err = motors_init();
if(init_err < 0) return init_err;

mutex mtr_mutex;

const struct motor_cmd null_cmd = {NAN, NAN, NAN};
Expand Down Expand Up @@ -514,69 +511,74 @@ int main(int argc, char** argv){
}
}

// This is the only thread that should ever query motors directly
thread mtr_thread([&mtr_mutex, &mtr_cmds, &mtr_t_last_update, &mtr_t_last_cmd, &mtr_last_cmd, &cur_pos]() {

const double mtr_refresh_t_ms = 100;

auto exec_cmds = [&](){
for(int a = 0; a < num_axis_t; ++a){
for(int r = 0; r < num_rod_t; ++r){
// Awkward to make sure thread safe
vector<function<void(void)>> moves;
{
lock_guard<mutex> lock(mtr_mutex);
motor_cmd cmd = mtr_cmds[a][r];
motor_cmd last_cmd = mtr_last_cmd[a][r];

if((!isnan(cmd.vel) && abs(cmd.vel - last_cmd.vel) > eps)
|| (!isnan(cmd.accel) && abs(cmd.accel - last_cmd.accel) > eps)){
moves.push_back([a, r, cmd](){
mtr_set_speed[a](r, cmd.vel, cmd.accel);
});
if(!isnan(cmd.vel))
mtr_last_cmd[a][r].vel = cmd.vel;
if(!isnan(cmd.accel))
mtr_last_cmd[a][r].accel = cmd.accel;
mtr_t_last_cmd[a][r] = mgr.TimeStampMsec();
if(!no_motors){
int init_err = motors_init();
if(init_err < 0) return init_err;

// This is the only thread that should ever query motors directly
thread mtr_thread([&mtr_mutex, &mtr_cmds, &mtr_t_last_update, &mtr_t_last_cmd, &mtr_last_cmd, &cur_pos]() {

const double mtr_refresh_t_ms = 100;

auto exec_cmds = [&](){
for(int a = 0; a < num_axis_t; ++a){
for(int r = 0; r < num_rod_t; ++r){
// Awkward to make sure thread safe
vector<function<void(void)>> moves;
{
lock_guard<mutex> lock(mtr_mutex);
motor_cmd cmd = mtr_cmds[a][r];
motor_cmd last_cmd = mtr_last_cmd[a][r];

if((!isnan(cmd.vel) && abs(cmd.vel - last_cmd.vel) > eps)
|| (!isnan(cmd.accel) && abs(cmd.accel - last_cmd.accel) > eps)){
moves.push_back([a, r, cmd](){
mtr_set_speed[a](r, cmd.vel, cmd.accel);
});
if(!isnan(cmd.vel))
mtr_last_cmd[a][r].vel = cmd.vel;
if(!isnan(cmd.accel))
mtr_last_cmd[a][r].accel = cmd.accel;
mtr_t_last_cmd[a][r] = mgr.TimeStampMsec();
}

if(!isnan(cmd.pos) && abs(cmd.pos - last_cmd.pos) > eps){
moves.push_back([a, r, cmd](){
mtr_move[a](r, cmd.pos);
});
mtr_last_cmd[a][r].pos = cmd.pos;
mtr_t_last_cmd[a][r] = mgr.TimeStampMsec();
}
}

if(!isnan(cmd.pos) && abs(cmd.pos - last_cmd.pos) > eps){
moves.push_back([a, r, cmd](){
mtr_move[a](r, cmd.pos);
});
mtr_last_cmd[a][r].pos = cmd.pos;
mtr_t_last_cmd[a][r] = mgr.TimeStampMsec();
// This is outside the lock's scope to avoid holding mutex too long
for(auto fn : moves){
fn();
}
}
// This is outside the lock's scope to avoid holding mutex too long
for(auto fn : moves){
fn();
}
}
}
};
for(ever){
for(int a = 0; a < num_axis_t; ++a){
for(int r = 0; r < num_rod_t; ++r){
exec_cmds();
if(mgr.TimeStampMsec() - mtr_t_last_update[a][r] > mtr_refresh_t_ms){
lock_guard<mutex> lock(mtr_mutex);
if(a == lin){
cur_pos[a][r] = abs(nodes[lin][r].get().Motion.PosnMeasured.Value()
/ lin_cm_to_cnts[r]);
};
for(ever){
for(int a = 0; a < num_axis_t; ++a){
for(int r = 0; r < num_rod_t; ++r){
exec_cmds();
if(mgr.TimeStampMsec() - mtr_t_last_update[a][r] > mtr_refresh_t_ms){
lock_guard<mutex> lock(mtr_mutex);
if(a == lin){
cur_pos[a][r] = abs(nodes[lin][r].get().Motion.PosnMeasured.Value()
/ lin_cm_to_cnts[r]);
} else {
cur_pos[a][r] = nodes[rot][r].get().Motion.PosnMeasured.Value()
/ rot_rad_to_cnts[r] / deg_to_rad;
}
mtr_t_last_update[a][r] = mgr.TimeStampMsec();
} else {
cur_pos[a][r] = nodes[rot][r].get().Motion.PosnMeasured.Value()
/ rot_rad_to_cnts[r] / deg_to_rad;
this_thread::sleep_for(chrono::microseconds(100));
}
mtr_t_last_update[a][r] = mgr.TimeStampMsec();
} else {
this_thread::sleep_for(chrono::microseconds(100));
}
}
}
}
});
});
}

/**************************************************************************
* Main Event Loop
Expand All @@ -587,8 +589,8 @@ int main(int argc, char** argv){
cout << endl;
cout << fixed << setprecision(2);

state_t state = state_controlled;
/* state_t state = state_defense; */
/* state_t state = state_controlled; */
state_t state = state_defense;
control_task_t control_task = control_task_init;
double control_task_timer = mgr.TimeStampMsec();
// 1 = right, -1 = left, 0 = not shooting
Expand Down Expand Up @@ -660,23 +662,24 @@ int main(int argc, char** argv){

double time_ms = mgr.TimeStampMsec();


if(controller){
for(int i = 0; i < num_rod_t; ++i){
double ws_pos, ws_pos_updated, ws_rot, ws_rot_updated;
{
lock_guard<mutex> lock(ws_mutex);
ws_pos = tgt_pos[i];
ws_pos_updated = tgt_pos_updated[i];
ws_rot = tgt_rot[i];
ws_rot_updated = tgt_rot_updated[i];
}
if(ws_pos_updated){
move_lin(i, lin_range_cm[i] * ws_pos);
ws_pos_updated = false;
for(int r = 0; r < num_rod_t; ++r){

lock_guard<mutex> lock(ws_mutex);
if(tgt_pos_updated[r]){
mtr_cmds[lin][r] = {
.pos = lin_range_cm[r] * tgt_pos[r],
.vel = 100,
.accel = 100,
};
}
if(ws_rot_updated){
move_rot(i, ws_rot / deg_to_rad);
ws_rot_updated = false;
if(tgt_rot_updated[r]){
mtr_cmds[rot][r] = {
.pos = tgt_rot[r] / deg_to_rad,
.vel = 500,
.accel = 1000,
};
}
}
// Yes, else switch is just as much as a thing as else if
Expand All @@ -686,7 +689,7 @@ int main(int argc, char** argv){
int front;
pair<side_t, rod_t> closest = closest_rod(ball_pos[1]);
if(closest.first == bot){
state = state_uncontrolled;
/* state = state_uncontrolled; */
break;
}
if(closest.second == three_bar) front = two_bar;
Expand Down Expand Up @@ -743,7 +746,7 @@ int main(int argc, char** argv){

// Hysteresis to prevent rapid commands
/* cout << cur_pos[lin][rod] << endl; */
if(move_cm > 0.5 && !no_motors){
if(move_cm > 0.5){
double accel = 1000 * clamp(move_cm / 2, 0.0, 1.0);
if(front < two_bar && rod >= two_bar){
accel = 50;
Expand Down Expand Up @@ -1153,7 +1156,7 @@ int main(int argc, char** argv){
break;
}

/* print_status(status.str(), true); */
print_status(status.str(), true);
/* cout << ball_pos_lcl[0] << ", " << ball_pos_lcl[1] << "; v: " << ball_vel_lcl[0] << ", " << ball_vel_lcl[1] << endl; */
/* cout << time_ms - start_t << endl; */

Expand Down
10 changes: 5 additions & 5 deletions software/physical_params.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,19 +120,19 @@ const double ball_rad = 3.475/2;
* Vision Parameters
******************************************************************************/

const double cal_offset[3] = {-3.7, -0.5, 0};
const double cal_offset[3] = {-4.5, -0.65, 2.1};
const int vision_fps = 200;

/******************************************************************************
* Motor Parameters
******************************************************************************/
const int lin_range_cnts[][2] = {
/* {-20200, 20}, */
{-20180, 20},
{-20190, 50},
/* {-10150, 20}, */
{-10130, 20},
{-30370, 20},
{21000, 20},
{-9990, 20},
{-31400, 20},
{16170, 20},
};

const int lin_mid_cnts[] = {
Expand Down

0 comments on commit d0bea1f

Please sign in to comment.