diff --git a/2016.1/Robot/examples/Robot/Robot.ino b/2016.1/Robot/examples/Robot/Robot.ino old mode 100644 new mode 100755 diff --git a/2016.1/Robot/keywords.txt b/2016.1/Robot/keywords.txt old mode 100644 new mode 100755 diff --git a/2016.1/Robot/robot.cpp b/2016.1/Robot/robot.cpp index 5135b87..2b924d1 100644 --- a/2016.1/Robot/robot.cpp +++ b/2016.1/Robot/robot.cpp @@ -1,5 +1,7 @@ #include "robot.h" + +// This constructor was designed to be used with the Duo-Driver (China) Robot::Robot(byte r_enable, byte r_motor_1, byte r_motor_2, @@ -13,10 +15,17 @@ Robot::Robot(byte r_enable, AutoControlBoard(r_enable, r_motor_1, r_motor_2, l_enable, l_motor_1, l_motor_2, r_vcc_ref, r_gnd_ref, - l_vcc_ref, l_gnd_ref) -{ + l_vcc_ref, l_gnd_ref){ } +// This constructor was designed to be used with the Duo-Driver (China) +// Robot::Robot(byte left_motor, byte right_motor): +// SensorBoard(), +// AutoControlBoard(left_motor, right_motor){ +// } + + + void Robot::useCommand(char command) { /* diff --git a/2016.1/Robot/robot.h b/2016.1/Robot/robot.h index 5b7dbc9..297bac0 100644 --- a/2016.1/Robot/robot.h +++ b/2016.1/Robot/robot.h @@ -17,6 +17,9 @@ class Robot: public SensorBoard, public AutoControlBoard byte r_vcc_ref=UNUSED, byte r_gnd_ref=UNUSED, byte l_vcc_ref=UNUSED, byte l_gnd_ref=UNUSED); + Robot(byte left_motor, byte right_motor); + + void useCommand(char command); }; diff --git a/2016.1/Trekking/trekking.cpp b/2016.1/Trekking/trekking.cpp index c40e86b..f3d98a0 100644 --- a/2016.1/Trekking/trekking.cpp +++ b/2016.1/Trekking/trekking.cpp @@ -311,14 +311,6 @@ void Trekking::trackTrajectory() { void Trekking::regulateControl() { - if(linear){ - desired_linear_velocity = MAX_LINEAR_VELOCITY; - desired_angular_velocity = 0; - }else{ - desired_linear_velocity = 0; - desired_angular_velocity = MAX_ANGULAR_VELOCITY; - } - //constants to handle errors const int k_rho = 1; const int k_gamma = 3; @@ -328,16 +320,17 @@ void Trekking::regulateControl() { // getting configurations (final/desired and current/real) Position* q_real = locator.getLastPosition(); //real robot position unsigned long t = tracking_regulation_timer.getElapsedTime(); - Position gap = q_real->calculateGap(targets.get(current_target_index)); + Position gap = q_real->calculateGap(*targets.get(current_target_index)); //Angular transformation - float rho = (gap.getX()^2 + gap.getY()^2)^(0.5); - float gamma = 0; // atan2(gap.getY(), gap.getX()) - q_real->getTheta() ????????????????? + + float rho = sqrt( pow(gap.getX(),2) + pow(gap.getY(),2) ); + float gamma = atan2(gap.getY(), gap.getX()) - q_real->getTheta(); float delta = -gamma - q_real->getTheta(); //Error Gain and v e w definition - v = (k_rho * rho) * cos(gamma); - w = (k_delta * delta + gamma) * (k_gamma * sin(gamma) * cos(gamma)/gamma); + float v = (k_rho * rho) * cos(gamma); + float w = (k_delta * delta + gamma) * (k_gamma * sin(gamma) * cos(gamma)/gamma); controlMotors(v, w, true); distance_to_target = q_real->distanceFrom(targets.get(current_target_index)); diff --git a/2016.1/Trekking/trekkingpins.h b/2016.1/Trekking/trekkingpins.h index 03c12cb..6881079 100644 --- a/2016.1/Trekking/trekkingpins.h +++ b/2016.1/Trekking/trekkingpins.h @@ -3,7 +3,7 @@ */ const byte INIT_BUTTON_PIN = 3; const byte EMERGENCY_BUTTON_PIN = 2; -const byte OPERATION_MODE_SWITCH_PIN = A3; \\ Não está atualizado! +const byte OPERATION_MODE_SWITCH_PIN = A3; // Não está atualizado! const bool MANUAL_MODE = 0; const bool AUTO_MODE = 1;