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

Add motor velocity / position check

Browse source
This commit is contained in:
AntoineH 2024-09-04 16:41:59 +02:00
parent 8d6f7db12a
commit d3ba2c1134
1 changed files with 17 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

21
src/cmd/motor/serve.go
View file

@ -126,17 +126,30 @@ func motor_sim(state_init State, config Config, sim_freq float64){
close(stateChan)
return
}
//Clamp cmd_vel to max/min limits
if cmd_vel > config.Max_vel || cmd_vel < -config.Max_vel {
log.Printf("Command velocity out of limits (%f). Clamping to %f",cmd_vel,config.Max_vel)
if cmd_vel>=0 {
cmd_vel = config.Max_vel
} else if cmd_vel < 0 {
cmd_vel = -config.Max_vel
}
}
break;
default: //No command, do nothing
}
// cmdVelChan
//Update state
state.Velocity = cmd_vel //Infinite acceleration
state.Angle += state.Velocity * 1/sim_freq //Neglect computation delay for delta time
//TODO: Error check
//TODO: Error recovery ?
if state.Error == "" { //If error, stop update
state.Velocity = cmd_vel //Infinite acceleration
state.Angle += state.Velocity * 1/sim_freq //Neglect computation delay for delta time
}
if state.Angle > config.Max_pos || state.Angle < config.Min_pos { //Limit angle
state.Error = "Position out of range"
}
//Send new state to channel (non-blocking)
select{
case stateChan <- state: