void odometry()
{
int encoder[2]={0,0}, prev_encoder[2] = {0,0}, dl, dr; // dl is change in left motor and dr is change in right motor
if(new_estimate)
{ // if new_estimate is true
x = x_est;
y = y_est;
t = t_est;
new_estimate = false;
}
encoder[0] = left_motor.position();
encoder[1] = right_motor.position();
dl = encoder[0]-prev_encoder[0];
dr = encoder[1]-prev_encoder[1]; // current position- prev position
x += cos(t)*speed*(dl+dr)/2.0;
y += sin(t)*speed*(dl+dr)/2.0; // avg. of dl and dr are takin i guess
t += speed*(dl-dr)/L; // t is theta
prev_encoder[0] = encoder[0];
prev_encoder[1] = encoder[1];
vl = left_motor.pulses_per_second(); // this function returns the velocity.
vr = right_motor.pulses_per_second();
vx = (vl+vr)/2*speed;
wt = (vl-vr)/L*speed;
}
void motor_action(motor &dev)
{
char c = 0;
int new_value = 0;
std::string answer;
bool running = false;
do
{
cout << endl
<< "*** " << dev.driver_name() << " motor (" << dev.port_name() << ") actions ***" << endl
<< endl
<< "(i)nfo" << endl
<< "(c)ommand" << endl
<< "st(o)p command [" << dev.stop_command() << "]" << endl
<< "speed r(e)gulation [" << dev.speed_regulation_enabled() << "]" << endl;
if (dev.speed_regulation_enabled()==dev.speed_regulation_on)
cout << "speed (s)etpoint (" << dev.speed_sp() << ")" << endl;
else
cout << "duty cycle (s)etpoint (" << dev.duty_cycle_sp() << ")" << endl;
cout << "(p)osition setpoint (" << dev.position_sp() << ")" << endl
<< "ramp (u)p setpoint (" << dev.ramp_up_sp() << ")" << endl
<< "ramp (d)own setpoint (" << dev.ramp_down_sp() << ")" << endl
<< "(t)ime setpoint (" << dev.time_sp() << ")" << endl
<< endl
<< "(0) reset position" << endl
<< endl
<< "(b)ack" << endl
<< endl
<< "Choice: ";
cin >> c;
switch (c)
{
case 'i':
cout << endl;
//~autogen cpp_generic_report_status classes.motor>currentClass
cout << " Commands: ";
try { cout << dev.commands() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Count Per Rot: ";
try { cout << dev.count_per_rot() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Driver Name: ";
try { cout << dev.driver_name() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Duty Cycle: ";
try { cout << dev.duty_cycle() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Duty Cycle SP: ";
try { cout << dev.duty_cycle_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Encoder Polarity: ";
try { cout << dev.encoder_polarity() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Polarity: ";
try { cout << dev.polarity() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Port Name: ";
try { cout << dev.port_name() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position: ";
try { cout << dev.position() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position P: ";
try { cout << dev.position_p() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position I: ";
try { cout << dev.position_i() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position D: ";
try { cout << dev.position_d() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position SP: ";
try { cout << dev.position_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed: ";
try { cout << dev.speed() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed SP: ";
try { cout << dev.speed_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Ramp Up SP: ";
try { cout << dev.ramp_up_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Ramp Down SP: ";
try { cout << dev.ramp_down_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed Regulation Enabled: ";
try { cout << dev.speed_regulation_enabled() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed Regulation P: ";
try { cout << dev.speed_regulation_p() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed Regulation I: ";
try { cout << dev.speed_regulation_i() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed Regulation D: ";
try { cout << dev.speed_regulation_d() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " State: ";
try { cout << dev.state() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Stop Command: ";
try { cout << dev.stop_command() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Stop Commands: ";
try { cout << dev.stop_commands() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Time SP: ";
try { cout << dev.time_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
//~autogen
cout << endl;
break;
case 'c':
cout << "command " << dev.commands() << ": ";
cin >> answer; dev.set_command(answer); cout << endl;
break;
case 'o':
cout << "stop command " << dev.stop_commands() << ": ";
cin >> answer; dev.set_stop_command(answer); cout << endl;
break;
case 'e':
cout << "speed regulation (off, on): ";
cin >> answer; dev.set_speed_regulation_enabled(answer); cout << endl;
break;
case 's':
if (dev.speed_regulation_enabled()==dev.speed_regulation_on)
{
cout << "speed: "; cin >> new_value; dev.set_speed_sp(new_value); cout << endl;
}
else
{
cout << "duty cycle: "; cin >> new_value; dev.set_duty_cycle_sp(new_value); cout << endl;
}
break;
case 'p':
cout << "position: "; cin >> new_value; dev.set_position_sp(new_value); cout << endl;
break;
case 'u':
cout << "ramp up: "; cin >> new_value; dev.set_ramp_up_sp(new_value); cout << endl;
break;
case 'd':
cout << "ramp down: "; cin >> new_value; dev.set_ramp_down_sp(new_value); cout << endl;
break;
case 't':
cout << "time: "; cin >> new_value; dev.set_time_sp(new_value); cout << endl;
break;
case '0':
dev.set_position(0);
break;
}
void motor_action(motor &dev)
{
char c = 0;
int new_value = 0;
std::string answer;
bool running = false;
do
{
cout << endl
<< "*** " << dev.driver_name() << " motor (" << dev.address() << ") actions ***" << endl
<< endl
<< "(i)nfo" << endl
<< "(c)ommand" << endl
<< "st(o)p command [" << dev.stop_action() << "]" << endl
<< "speed (s)etpoint (" << dev.speed_sp() << ")" << endl;
cout << "(p)osition setpoint (" << dev.position_sp() << ")" << endl
<< "ramp (u)p setpoint (" << dev.ramp_up_sp() << ")" << endl
<< "ramp (d)own setpoint (" << dev.ramp_down_sp() << ")" << endl
<< "(t)ime setpoint (" << dev.time_sp() << ")" << endl
<< endl
<< "(0) reset position" << endl
<< endl
<< "(b)ack" << endl
<< endl
<< "Choice: ";
cin >> c;
switch (c)
{
case 'i':
cout << endl;
//~autogen generic_report_status classes.motor>currentClass
cout << " Address: ";
try { cout << dev.address() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Commands: ";
try { cout << dev.commands() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Count Per Rot: ";
try { cout << dev.count_per_rot() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Count Per M: ";
try { cout << dev.count_per_m() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Driver Name: ";
try { cout << dev.driver_name() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Duty Cycle: ";
try { cout << dev.duty_cycle() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Duty Cycle SP: ";
try { cout << dev.duty_cycle_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Full Travel Count: ";
try { cout << dev.full_travel_count() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Polarity: ";
try { cout << dev.polarity() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position: ";
try { cout << dev.position() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position P: ";
try { cout << dev.position_p() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position I: ";
try { cout << dev.position_i() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position D: ";
try { cout << dev.position_d() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Position SP: ";
try { cout << dev.position_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Max Speed: ";
try { cout << dev.max_speed() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed: ";
try { cout << dev.speed() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed SP: ";
try { cout << dev.speed_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Ramp Up SP: ";
try { cout << dev.ramp_up_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Ramp Down SP: ";
try { cout << dev.ramp_down_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed P: ";
try { cout << dev.speed_p() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed I: ";
try { cout << dev.speed_i() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Speed D: ";
try { cout << dev.speed_d() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " State: ";
try { cout << dev.state() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Stop Action: ";
try { cout << dev.stop_action() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Stop Actions: ";
try { cout << dev.stop_actions() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
cout << " Time SP: ";
try { cout << dev.time_sp() << endl; }
catch(...) { cout << "[" << strerror(errno) << "]" << endl; }
//~autogen
cout << endl;
break;
case 'c':
cout << "command " << dev.commands() << ": ";
cin >> answer; dev.set_command(answer); cout << endl;
break;
case 'o':
cout << "stop command " << dev.stop_actions() << ": ";
cin >> answer; dev.set_stop_action(answer); cout << endl;
break;
case 's':
cout << "speed: "; cin >> new_value; dev.set_speed_sp(new_value); cout << endl;
break;
case 'p':
cout << "position: "; cin >> new_value; dev.set_position_sp(new_value); cout << endl;
break;
case 'u':
cout << "ramp up: "; cin >> new_value; dev.set_ramp_up_sp(new_value); cout << endl;
break;
case 'd':
cout << "ramp down: "; cin >> new_value; dev.set_ramp_down_sp(new_value); cout << endl;
break;
case 't':
cout << "time: "; cin >> new_value; dev.set_time_sp(new_value); cout << endl;
break;
case '0':
dev.set_position(0);
break;
}
}
while (c != 'b');
}
