void MainWindow::SendCAN(void)
{
char tmp[300] = "";
string can = "";
// add time when sent out.
sprintf(tmp, "%d,'%d/%02d/%02d %02d:%02d:%02d.%ld'",
CAN_KEY_TIME,
_s_time->tm_year+1900, _s_time->tm_mon+1, _s_time->tm_mday,
_s_time->tm_hour + 9, _s_time->tm_min, _s_time->tm_sec,
_getTime.tv_usec);
can += tmp;
if (_selectLog.drvInf == true) {
sprintf(tmp, ",%d,%3.2f,%d,%d,%d,%d",
CAN_KEY_VELOC, _drvInf.veloc,
CAN_KEY_ACCEL, _drvInf.actualPedalStr,
CAN_KEY_SHIFT, _drvInf.actualShift);
can += tmp;
}
if (_selectLog.strInf == true) {
sprintf(tmp, ",%d,%3.2f,%d,%d",
CAN_KEY_ANGLE, _strInf.angle,
CAN_KEY_TORQUE, _strInf.torque);
can += tmp;
}
if (_selectLog.brkInf == true) {
sprintf(tmp, ",%d,%d", CAN_KEY_BRAKE, _brakeInf.actualPedalStr);
can += tmp;
}
candata = can;
// send drive mode in addition to CAN.
UpdateState();
if (ZMP_DRV_CONTROLLED() && ZMP_STR_CONTROLLED()) {
drvmode = CMD_MODE_PROGRAM;
} else {
drvmode = CMD_MODE_MANUAL;
}
wrapSender();
}
void MainWindow::StrokeControl(double current_velocity, double cmd_velocity)
{
static queue<double> vel_buffer;
static uint vel_buffer_size = 10;
double old_velocity = 0.0;
// don't control if not in program mode.
if (!ZMP_DRV_CONTROLLED()) {
clear_diff();
return;
}
// estimate current acceleration.
vel_buffer.push(current_velocity);
if (vel_buffer.size() > vel_buffer_size) {
old_velocity = vel_buffer.front();
vel_buffer.pop(); // remove old_velocity from the queue.
estimate_accel =
(current_velocity-old_velocity)/(cycle_time*vel_buffer_size);
}
cout << "estimate_accel: " << estimate_accel << endl;
if (fabs(cmd_velocity) >= current_velocity
&& fabs(cmd_velocity) > 0.0
&& current_velocity <= SPEED_LIMIT) {
double accel_stroke;
cout << "accelerate: current_velocity=" << current_velocity
<< ", cmd_velocity=" << cmd_velocity << endl;
accel_stroke = _accel_stroke_pid_control(current_velocity, cmd_velocity);
if (accel_stroke < 0) {
cout << "ZMP_SET_BRAKE_STROKE(" << -accel_stroke << ")" << endl;
ZMP_SET_BRAKE_STROKE(-accel_stroke);
}
else {
cout << "ZMP_SET_DRV_STROKE(" << accel_stroke << ")" << endl;
ZMP_SET_DRV_STROKE(accel_stroke);
}
}
else if (fabs(cmd_velocity) < current_velocity
&& fabs(cmd_velocity) > 0.0) {
double brake_stroke;
cout << "decelerate: current_velocity=" << current_velocity
<< ", cmd_velocity=" << cmd_velocity << endl;
brake_stroke = _brake_stroke_pid_control(current_velocity, cmd_velocity);
if (brake_stroke < 0) {
cout << "ZMP_SET_DRV_STROKE(" << -brake_stroke << ")" << endl;
ZMP_SET_DRV_STROKE(-brake_stroke);
}
else {
cout << "ZMP_SET_BRAKE_STROKE(" << brake_stroke << ")" << endl;
ZMP_SET_BRAKE_STROKE(brake_stroke);
}
}
else if (cmd_velocity == 0.0 && current_velocity != 0.0) {
double brake_stroke;
cout << "stopping: current_velocity=" << current_velocity
<< ", cmd_velocity=" << cmd_velocity << endl;
if (current_velocity < 3.0) { // nearly stopping
ZMP_SET_DRV_STROKE(0);
brake_stroke = _stopping_control(current_velocity);
ZMP_SET_BRAKE_STROKE(brake_stroke);
}
else {
brake_stroke = _brake_stroke_pid_control(current_velocity, 0);
if (brake_stroke < 0) {
cout << "ZMP_SET_DRV_STROKE(" << -brake_stroke << ")" << endl;
ZMP_SET_DRV_STROKE(-brake_stroke);
}
else {
cout << "ZMP_SET_BRAKE_STROKE(" << brake_stroke << ")" << endl;
ZMP_SET_BRAKE_STROKE(brake_stroke);
}
}
}
else {
cout << "unknown: current_velocity=" << current_velocity
<< ", cmd_velocity=" << cmd_velocity << endl;
}
}
