//*****************************************************************************
//
//! Move robot a little bit (error +-500 pulses). Robot velocity is equal 0 after moving.
//! Make sure this function return true before calling it again.
//!
//! \param deltaLeft distance left motor will go
//! \param deltaRight distance right motor will go
//! \param velLeftMax max left velocity
//! \param velRightMax max right velocity
//!
//! \return true if done
//
//*****************************************************************************
static bool move(int deltaLeft,int deltaRight,int velLeftMax, int velRightMax)
{
static int origLeft, origRight;
static bool done = true;
int currentLeft=qei_getPosLeft();
int currentRight=qei_getPosRight();
if (done)
{
done=false;
origLeft=currentLeft;
origRight=currentRight;
}
//bluetooth_print("move: %5d %5d\r\n",origLeft,currentLeft);
if (abs(origLeft+deltaLeft-currentLeft)>500)
{
pid_posLeft.PID_Saturation = velLeftMax;
speed_set(MOTOR_LEFT, pid_process(&pid_posLeft,origLeft+deltaLeft-currentLeft));
pid_posRight.PID_Saturation = velRightMax;
speed_set(MOTOR_RIGHT, pid_process(&pid_posRight,origRight+deltaRight-currentRight));
done=false;
}
else
{
done = true;
pid_reset(&pid_posLeft);
pid_reset(&pid_posRight);
}
#ifdef _DEBUG_MOVE
bluetooth_print("move: %5d %5d %5d %5d\r\n",(int)left, (int)right,(int)pid_posLeft.u,(int)pid_posRight.u);
#endif
return done;
}
uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy){
float temperature = sensor->get_temperature();
if (target_temperature > 0)
{
if (isinf(temperature))
{
this->min_temp_violated = true;
target_temperature = UNDEFINED;
heater_pin.set((this->o=0));
}
else
{
pid_process(temperature);
if ((temperature > target_temperature) && waiting)
{
THEKERNEL->pauser->release();
waiting = false;
}
}
}
else
{
heater_pin.set((this->o = 0));
}
last_reading = temperature;
return 0;
}
/**
* @brief Wall follow controller
*/
static bool pid_wallfollow(float delta_IR_left, float delta_IR_right, float averageSpeed)
{
static float error, u;
int32_t set_speed[2];
switch (e_wall_follow_select)
{
case WALL_FOLLOW_NONE: //Do nothing
return true;
case WALL_FOLLOW_LEFT:
error = delta_IR_left;
break;
case WALL_FOLLOW_RIGHT:
error = delta_IR_right;
break;
case WALL_FOLLOW_BOTH:
error = delta_IR_left - delta_IR_right;
break;
default:
return false;
}
u = pid_process(error);
set_speed[0] = averageSpeed + (int32_t)(u / 2);
set_speed[1] = averageSpeed - (int32_t)(u / 2);
speed_set(MOTOR_RIGHT, set_speed[0]);
speed_set(MOTOR_LEFT, set_speed[1]);
return true;
}
float pid_hum(float eLast)
{
float pid_value=0;
pid_Eupdate(&HumPID, eLast);
pid_value = pid_process(&HumPID);
return pid_value;
}
void pid_cascade_control(struct pid_cascade_s *ctrl)
{
// position control
float pos_ctrl_vel;
if (ctrl->setpts.position_control_enabled) {
ctrl->position_setpoint = ctrl->setpts.position_setpt;
float position_error = ctrl->position - ctrl->setpts.position_setpt;
if (ctrl->periodic_actuator) {
position_error = periodic_error(position_error);
}
ctrl->position_error = position_error;
pos_ctrl_vel = pid_process(&ctrl->position_pid, ctrl->position_error);
ctrl->position_ctrl_out = pos_ctrl_vel;
} else {
pid_reset_integral(&ctrl->position_pid);
pos_ctrl_vel = 0;
}
// velocity control
float vel_ctrl_torque;
if (ctrl->setpts.velocity_control_enabled) {
float velocity_setpt = ctrl->setpts.velocity_setpt + pos_ctrl_vel;
velocity_setpt = filter_limit_sym(velocity_setpt, ctrl->velocity_limit);
ctrl->velocity_setpoint = velocity_setpt;
ctrl->velocity_error = ctrl->velocity - velocity_setpt;
vel_ctrl_torque = pid_process(&ctrl->velocity_pid, ctrl->velocity_error);
ctrl->velocity_ctrl_out = vel_ctrl_torque;
} else {
pid_reset_integral(&ctrl->velocity_pid);
vel_ctrl_torque = 0;
}
// torque control
float torque_setpt = vel_ctrl_torque + ctrl->setpts.feedforward_torque;
torque_setpt = filter_limit_sym(torque_setpt, ctrl->torque_limit);
float current_setpt = torque_setpt * ctrl->motor_current_constant;
current_setpt = filter_limit_sym(current_setpt, ctrl->current_limit);
ctrl->current_setpoint = current_setpt;
ctrl->current_error = ctrl->current - current_setpt;
ctrl->motor_voltage = pid_process(&ctrl->current_pid, ctrl->current_error);
}
void
cos_init(void *arg)
{
int c, accept_fd, ret;
long eid;
char *init_str = cos_init_args();
char __create_str[128];
static volatile int first = 1, off = 0;
int port;
u64_t start, end;
if (cos_get_thd_id() == pid_thd) {
pid_process();
}
union sched_param sp;
if (first) {
first = 0;
sp.c.type = SCHEDP_PRIO;
sp.c.value = 10;
pid_thd = sched_create_thd(cos_spd_id(), sp.v, 0, 0);
init(init_str);
return;
}
printc("Thread %d, port %d\n", cos_get_thd_id(), __port+off);
port = off++;
port += __port;
eid = evt_get();
if (snprintf(__create_str, 128, create_str, port) < 0) BUG();
ret = c = from_tsplit(cos_spd_id(), td_root, __create_str, strlen(__create_str), TOR_ALL, eid);
if (ret <= td_root) BUG();
accept_fd = c;
printc("accept_fd %d (eid %d)\n", accept_fd, eid);
evt_add(c, eid);
/* event loop... */
while (1) {
int t;
long evt;
evt = evt_wait_all();
t = evt_torrent(evt);
printc("an interrupt comes in (thd %d, evt %d t %d)\n",
cos_get_thd_id(), evt, t);
accept_new(accept_fd);
break;
}
}
uint32_t TemperatureControl::thermistor_read_tick(uint32_t dummy)
{
float temperature = sensor->get_temperature();
if(!this->readonly && target_temperature > 2) {
if (isinf(temperature) || temperature < min_temp || temperature > max_temp) {
this->temp_violated = true;
target_temperature = UNDEFINED;
heater_pin.set((this->o = 0));
} else {
pid_process(temperature);
}
}
last_reading = temperature;
return 0;
}
/**
* @brief Wall follow controller
*/
static bool pid_wallfollow(float delta_IR_left, float delta_IR_right, float averageSpeed,
WALL_FOLLOW_SELECT wall_follow_select)
{
static float error, u, rightFirst=1;
static WALL_FOLLOW_SELECT preSelect=WALL_FOLLOW_NONE;
int32_t set_speed[2];
if (preSelect!=WALL_FOLLOW_NONE)
if (preSelect!=wall_follow_select)
{
if (preSelect==WALL_FOLLOW_RIGHT)
pid_reset(&pid_wall_right);
else if (preSelect==WALL_FOLLOW_LEFT)
pid_reset(&pid_wall_left);
}
switch (wall_follow_select)
{
case WALL_FOLLOW_NONE: //Do nothing
return true;
case WALL_FOLLOW_LEFT:
{
error = -delta_IR_left;
u = pid_process(&pid_wall_left,error);
}
break;
case WALL_FOLLOW_RIGHT:
{
error = delta_IR_right;
u = pid_process(&pid_wall_right,error);
}
break;
case WALL_FOLLOW_BOTH:
{
error = delta_IR_right - delta_IR_left;
u = pid_process(&pid_wall_right,error);
}
break;
case WALL_FOLLOW_AUTO:
if (rightFirst)
{
if (isWallRight)
{
error = delta_IR_right;
u = pid_process(&pid_wall_right,error);
}
else if (isWallLeft)
{
pid_reset(&pid_wall_right);
error = -delta_IR_left;
u = pid_process(&pid_wall_left,error);
rightFirst=0;
}
}
else
{
if (isWallLeft)
{
error = -delta_IR_left;
u = pid_process(&pid_wall_left,error);
}
else if (isWallRight)
{
pid_reset(&pid_wall_left);
error = delta_IR_right;
u = pid_process(&pid_wall_right,error);
rightFirst=1;
}
}
break;
default:
return false;
}
preSelect = wall_follow_select;
set_speed[0] = averageSpeed + (int32_t)(u / 2);
set_speed[1] = averageSpeed - (int32_t)(u / 2);
speed_set(MOTOR_RIGHT, set_speed[0]);
speed_set(MOTOR_LEFT, set_speed[1]);
return true;
}
