Debug

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)
 {
 	/*

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);
 };
 

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));

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;