void CylindersModel::update()
{
setDive(current);
}
void WeightModel::update()
{
setDive(current);
}
void NavigationControl::PointCallback(const Robosub::Point& msg)
{
// The speed is created based on the difference between
// the target point and the center
// If the it is the beginning of the process
// save the target as the objective
// Target points
int target_x = msg.x;
int target_y = msg.y;
if (!target_x && !target_y ) {
// Centered = true;
Begun = false;
start_x = 0;
start_y = 0;
setStrafe(0);
setDive(0);
return;
}
if (!Begun){ // First assignment: create starting points
start_x = msg.x;
start_y = msg.y;
Begun = true;
// Centered = false;
}
// Calculate the directions what direction do we have to move?
int dir_x = target_x>0 ? 1 : -1; //Left -1, Right +1
int dir_y = target_y>0 ? -1 : 1; //Up -1, Down +1
// Calculate the percentage (OR SIMPLY USE ERROR??)
float per_x = (target_x/240); //start_x);
float per_y = (target_y/320); //start_y);
// Control Process (calculate the right thrust)
// Use less than 100% based on how big the number is.
// Convert the distance percentage to thrust percentage
// These are set assuming there is no bias on the voltage
// sent to the thrusters
int minT = 50; // to avoid sending values under the minimum.
//maxT = 90; //To avoid moving too fast and sucking too much power
int range = 40;
int thrust_x = 0;
int thrust_y = 0;
if((per_x) <MAX_THRESHOLD){
if((per_x)>MIN_THRESHOLD){ //Send a negative thrust to decrease the speed
thrust_x = -60*dir_x;
} else {
thrust_x = 0;
}
} else {
thrust_x = (range*per_x+minT)*dir_x;
}
if((per_y) <MAX_THRESHOLD){
if((per_y)>MIN_THRESHOLD){ //Send a negative thrust to decrease the speed
thrust_y = -60*dir_y;
} else {
thrust_y = 0; // We zeroed out in this direction
}
} else {
if(dir_y>0){ // && FORWARDCAMERA
range = 30; //So we don't go up too fast
thrust_y = (range*per_y+minT)*dir_y;
}
}
//Send the calculated speed to the motor
setStrafe(thrust_x);
setDive(thrust_y);
}
