AntoineH/multi-motors-controller
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Controller handle no movement case

Browse source
This commit is contained in:
AntoineH 2024-09-05 11:33:19 +02:00
parent bcbf072eb4
commit c20f748cee
1 changed files with 33 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

50
src/cmd/controller/serve.go
View file

@ -62,11 +62,12 @@ type server struct {
} }
//TODO: Coroutine to regulate motor velocities //TODO: Coroutine to regulate motor velocities
//TODO: Minimal trajectory time (eg 1s) to avoid jerks/overshoots
func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, error) { func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, error) {
//TODO: use as a coroutine to prevent blocking the main thread //TODO: use as a coroutine to prevent blocking the main thread
var( var(
tgt_angles = []float64{in.GetAngles()[0], in.GetAngles()[0], in.GetAngles()[0]} //TODO: Replace by in.GetAngles() tgt_angles = []float64{in.GetAngles()[0], in.GetAngles()[0], in.GetAngles()[0]} //TODO: Replace by in.GetAngles()
cmd_vel = []float64{} //Commanded velocities cmd_vel = make([]float64, len(tgt_angles)) //Commanded velocities
motor_pos = []float64{} //Current motor positions motor_pos = []float64{} //Current motor positions
max_vels = []float64{} //Maximum velocities max_vels = []float64{} //Maximum velocities
traj_times = []float64{} //Trajectory times traj_times = []float64{} //Trajectory times
@ -75,6 +76,7 @@ func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, erro
motor_errors = []string{} //Motor errors message motor_errors = []string{} //Motor errors message
controller_error string = "" //Controller error type controller_error string = "" //Controller error type
) )
//Get motor states //Get motor states
for i := range tgt_angles { for i := range tgt_angles {
//TODO: Use a coroutine to avoid blocking the main thread //TODO: Use a coroutine to avoid blocking the main thread
@ -100,40 +102,49 @@ func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, erro
//Compute velocities //Compute velocities
if controller_error ==""{ //If no error, compute velocities if controller_error ==""{ //If no error, compute velocities
//Compute minimum trajectory times
for i := range tgt_angles { for i := range tgt_angles {
//Compute minimum trajectory times delta_pos := math.Abs(tgt_angles[i]-motor_pos[i])
traj_times = append(traj_times, math.Abs(tgt_angles[i]-motor_pos[i])/max_vels[i]) if delta_pos>0 { //If there is a movevement to be done, compute trajectory time
traj_times = append(traj_times, delta_pos/max_vels[i])
}
} }
//Compute maximal velocities //Compute maximal velocities
s_traj_t = slices.Min(traj_times) //Minimum trajectory time if len(traj_times)>0 { //If there is a trajectory to do, compute maximal velocities
//TODO: Prevent NaN in case of 0 trajectory time s_traj_t = slices.Min(traj_times) //Minimum trajectory time
for i := range tgt_angles { for i := range tgt_angles {
cmd_vel = append(cmd_vel, (tgt_angles[i]-motor_pos[i])/s_traj_t) cmd_vel[i] =(tgt_angles[i]-motor_pos[i])/s_traj_t
if math.Abs(cmd_vel[i]) > max_vels[i] { // Velocity needs to be limited (Flag) if math.Abs(cmd_vel[i]) > max_vels[i] { // Velocity needs to be limited (Flag)
limit_vel = true limit_vel = true
}
} }
} // log.Printf("cmd_vel: %v - %v", cmd_vel, limit_vel)
// log.Printf("cmd_vel: %v - %v", cmd_vel, limit_vel) if limit_vel { //Limit velocity to slowest motor
if limit_vel { //Limit velocity to slowest motor s_traj_t = slices.Max(traj_times) //Trajectory time of slowest motor
s_traj_t = slices.Max(traj_times) //Trajectory time of slowest motor for i := range cmd_vel {
for i := range cmd_vel { cmd_vel[i]= (tgt_angles[i]-motor_pos[i])/s_traj_t
cmd_vel[i]= (tgt_angles[i]-motor_pos[i])/s_traj_t }
} }
} else { //Nothing to do
controller_error = "NoTrajectory"
} }
} }
switch controller_error { switch controller_error {
case "MotorFault": case "MotorFault":
log.Printf("Motor error preventing command: %v", motor_errors) log.Printf("ERROR - Motor error preventing command: %v", motor_errors)
return &pb.Angles{Angles: motor_pos}, nil //Return motor positions TODO: Return error message return &pb.Angles{Angles: motor_pos}, nil //Return motor positions TODO: Return error message
case "InvalidTargetAngles": case "InvalidTargetAngles":
log.Print("Invalid target angles:") log.Print("ERROR - out of limits:")
for i, tgt := range tgt_angles{ for i, tgt := range tgt_angles{
log.Printf(" Motor %d: %f < [%f] < %f", motor_configs[i].Id, motor_configs[i].Min_pos, tgt, motor_configs[i].Max_pos) log.Printf(" Motor %d: %f < [%f] < %f", motor_configs[i].Id, motor_configs[i].Min_pos, tgt, motor_configs[i].Max_pos)
} }
return &pb.Angles{Angles: motor_pos}, nil //Return motor positions TODO: Return error message return &pb.Angles{Angles: motor_pos}, nil //Return motor positions TODO: Return error message
case "NoTrajectory":
log.Print("Already at target positions")
return &pb.Angles{Angles: motor_pos}, nil
default: //No error default: //No error
//Send command to motors //Send command to motors
log.Printf("Requested joints velocities (%v s): %v", s_traj_t, cmd_vel) log.Printf("Requested joints velocities (%v s): %v", s_traj_t, cmd_vel)
@ -143,6 +154,8 @@ func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, erro
//Stop motors after trajectory time //Stop motors after trajectory time
go stopMotors(time.Duration(float64(time.Second)*s_traj_t)) go stopMotors(time.Duration(float64(time.Second)*s_traj_t))
log.Printf("Motion in progress...")
return &pb.Angles{Angles: tgt_angles}, nil return &pb.Angles{Angles: tgt_angles}, nil
} }
} }
@ -153,6 +166,7 @@ func stopMotors(delay time.Duration) {
for i := range motor_configs { for i := range motor_configs {
setMotorVel(i, 0) setMotorVel(i, 0)
} }
log.Printf("SUCCESS") //TODO : Check state of motors for errors
} }
func setMotorVel(idx int, vel float64){ func setMotorVel(idx int, vel float64){