void ForwardSimple()
{
static int g_loop = 0;
int power = 60; // power
int power2 = 30; // turn power
// static int target = getCompass();
if (g_loop == 0) g_target = getCompass();
g_diff = g_target - getCompass();
if(g_diff > 180) g_diff -= 360;
if(g_diff < - 180) g_diff += 360;
int val = g_diff / 5;
if (g_ball[0] > SHOOT_THRESH) kick();
int angle = getBallDir();
if (g_diff > 30 || g_diff < -30) turn(val);
else
{
if (angle != 999)
{
if (g_use_lcd==1)
{
slcd.setCursor(0,0);
slcd.print("Move:");
}
if ((0 <= angle && angle < 60) || (300 <= angle) && (angle < 360))
{
moveAngle(power, 0, angle);
}
else if ((60 <= angle && angle < 135) || (225 <= angle && angle < 300))
{
moveAngle(power, 0, 180);
}
else if ((135 <= angle && angle < 180) || (180 <= angle && angle < 225))
{
if (g_dist[1] < g_dist[3]) moveAngle(power, 0, 270 - abs(angle - 180));
else moveAngle(power, 0, 90 + abs(angle - 180));
}
}
else
{
if (g_use_lcd==1)
{
slcd.setCursor(0,0);
slcd.print("Home:");
}
// go home
moveAngle(power, 0, 180);
}
}
g_loop++;
}
void loop()
{
static uint32_t updelay;
static float xpos, ypos, theta;
static int32_t lastlenc, lastrenc;
const uint32_t curtime = millis();
const float PI2 = PI * 2.0;
if (updelay < curtime)
{
#if 0
if (updelay == 0) // Init
{
getCompass().read();
theta = (getCompass().heading() * PI / 180.0);
}
updelay = curtime + 500;
const int32_t le = ((encoders.getDist(ENC_LB) + encoders.getDist(ENC_LF)) / 2);
const int32_t re = ((encoders.getDist(ENC_RB) + encoders.getDist(ENC_RF)) / 2);
const int32_t lticks = le - lastlenc, rticks = re - lastrenc;
lastlenc = le;
lastrenc = re;
const float ldist = lticks / ENC_PULSES_CM, rdist = rticks / ENC_PULSES_CM;
// theta += (((float)(lticks - rticks) / ENC_PULSES_DEG / 2.0) * PI / 180.0);
theta += ((ldist - rdist) / 2.0 / wheelBase);
// Clamp
while (true)
{
if (theta > PI2)
theta -= PI2;
else if (theta < 0)
theta += PI2;
else
break;
}
// theta -= ((float)((int)(theta/M_2_PI)) * M_2_PI);
const float dist = (ldist + rdist) / 2.0;
xpos += dist * sin(theta);
ypos += dist * cos(theta);
#endif
Serial.print("X/Y/theta: ");
Serial.print(encoders.getXPos()); Serial.print(", ");
Serial.print(encoders.getYPos()); Serial.print(", ");
Serial.println(encoders.getRotation());
}
rover5Task();
}
void PID()
{
int last_diff = g_diff;
static int target = getCompass();
g_diff = target - getCompass();
if(g_diff > 180) g_diff -= 360;
else if(g_diff < - 180) g_diff += 360;
float pro = g_diff / 5.4;
g_pid = pro + 7;
}
void PID()
{
int last_diff = g_diff;
static int target = getCompass();
g_diff = target - getCompass();
if(g_diff > 180) g_diff -= 360;
else if(g_diff < - 180) g_diff += 360;
float pro = g_diff * 1 / 4.8;
static float g_integral = 0;
if (last_diff + 2 <= g_diff) g_integral++;
if (g_diff < 0) g_integral *= -1;
g_pid = pro + g_integral ;
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
_distance_sensor[0] = getDistanceSensor("ds0");
_distance_sensor[0]->enable(_time_step);
_distance_sensor[1] = getDistanceSensor("ds15");
_distance_sensor[1]->enable(_time_step);
_distance_sensor[2] = getDistanceSensor("ds1");
_distance_sensor[2]->enable(_time_step);
_distance_sensor[3] = getDistanceSensor("ds14");
_distance_sensor[3]->enable(_time_step);
_distance_sensor[4] = getDistanceSensor("ds2");
_distance_sensor[4]->enable(_time_step);
_distance_sensor[5] = getDistanceSensor("ds13");
_distance_sensor[5]->enable(_time_step);
_distance_sensor[6] = getDistanceSensor("ds3");
_distance_sensor[6]->enable(_time_step);
_distance_sensor[7] = getDistanceSensor("ds12");
_distance_sensor[7]->enable(_time_step);
}
//////////////////////////////////////////////
// Constructor of the class robot
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
// Get the distance sensors
_dsensors[0] = getDistanceSensor("ds0");
_dsensors[1] = getDistanceSensor("ds1");
_dsensors[2] = getDistanceSensor("ds2");
_dsensors[3] = getDistanceSensor("ds3");
_dsensors[4] = getDistanceSensor("ds12");
_dsensors[5] = getDistanceSensor("ds13");
_dsensors[6] = getDistanceSensor("ds14");
_dsensors[7] = getDistanceSensor("ds15");
// Enable all distance sensors
for (int i = 0; i < NUMBER_OF_DSENSORS; i++)
{
_dsensors[i]->enable(_time_step);
}
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 50;
_left_speed = 0;
_right_speed = 0;
_mode = FORWARD;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
// Get and enable the distance sensors
_distance_sensor[0] = getDistanceSensor("ds0");
_distance_sensor[1] = getDistanceSensor("ds1");
_distance_sensor[2] = getDistanceSensor("ds2");
_distance_sensor[3] = getDistanceSensor("ds3");
_distance_sensor[4] = getDistanceSensor("ds4");
_distance_sensor[5] = getDistanceSensor("ds5");
_distance_sensor[6] = getDistanceSensor("ds6");
_distance_sensor[7] = getDistanceSensor("ds7");
_distance_sensor[8] = getDistanceSensor("ds8");
_distance_sensor[9] = getDistanceSensor("ds9");
_distance_sensor[10] = getDistanceSensor("ds10");
_distance_sensor[11] = getDistanceSensor("ds11");
_distance_sensor[12] = getDistanceSensor("ds12");
_distance_sensor[13] = getDistanceSensor("ds13");
_distance_sensor[14] = getDistanceSensor("ds14");
_distance_sensor[15] = getDistanceSensor("ds15");
for (int i=0; i<NUM_DISTANCE_SENSOR; i++) {
_distance_sensor[i]->enable(_time_step);
}
}
/**
* Maintains heading while moving forward by slowing down inner tread and
* speeding up outside tread in proportion to angular error from desired heading
**/
void Ant::driftCorrection(float direction) {
int allowedOffset;
//Angle offset from correct heading
float angle = util->angle(getCompass(), direction);
//If using motion capture to control robot
if(*motionCapture) {
//Use a looser offset to compensate for delay
allowedOffset = 5;
}
//Otherwise
else {
//Use tight offset
allowedOffset = 1;
}
if(angle >= allowedOffset) {
move->forward(*travelSpeed,constrain(*travelSpeed - (angle * 25), 0, 255));
}
else if(angle <= -allowedOffset) {
move->forward(constrain(*travelSpeed + (angle * 25), 0, 255), *travelSpeed);
}
else {
move->forward(*travelSpeed,* travelSpeed);
}
}
//////////////////////////////////////////////
//Definition of the constructor MyRobot where initialize the parameters of this program
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
}
// 地磁気センサのチェック
void checkCompass()
{
int dir;
// get Diretion
dir = getCompass();
if (g_use_lcd==1)
{
slcd.setCursor(0,0);
slcd.print(" Dir:");
slcd.print(dir,DEC);
slcd.print(" ");
}
}
MyRobot::MyRobot() : DifferentialWheels()
{
// init default values
_time_step = 64;
//init velocities of each wheel
_left_speed = 0;
_right_speed = 0;
//Get and enable the spherical camera proccesses
_spherical_camera = getCamera("camera_s");
_spherical_camera->enable(_time_step);
//Get and enable the compass sensor
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass -> enable(_time_step);
// Eneable and get encoders
enableEncoders(_time_step);
_left_encoder = getLeftEncoder();
_right_encoder = getRightEncoder();
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
// Initialize _posicion_final and _Radio.
_posicion_final = 0.0;
_Radio = 0.0825;
enableEncoders(_time_step);
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
_mode = FORWARD;
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
_distance_sensor[0] = getDistanceSensor("ds1");
_distance_sensor[0]->enable(_time_step);
_distance_sensor[1] = getDistanceSensor("ds14");
_distance_sensor[1]->enable(_time_step);
_distance_sensor[2] = getDistanceSensor("ds3");
_distance_sensor[2]->enable(_time_step);
_distance_sensor[3] = getDistanceSensor("ds12");
_distance_sensor[3]->enable(_time_step);
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
_mode =GOING_TO_INITIAL_POINT ;
// Get and enable the compass device and the distance sensors
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
_distance_sensor[0] = getDistanceSensor("ds0");
_distance_sensor[0]->enable(_time_step);
_distance_sensor[1] = getDistanceSensor("ds2");
_distance_sensor[1]->enable(_time_step);
_distance_sensor[2] = getDistanceSensor("ds14");
_distance_sensor[2]->enable(_time_step);
_distance_sensor[3] = getDistanceSensor("ds1");
_distance_sensor[3]->enable(_time_step);
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Initial movement and state mode
_mov_mode = FORWARD;
_robot_state = FOLLOWING_OBJETIVE;
//Get and enable the compass
_objetive_compass = getCompass("compass");
_objetive_compass->enable(_time_step);
// Get the distance sensors
_dsensors[0] = getDistanceSensor("ds0");
_dsensors[1] = getDistanceSensor("ds1");
_dsensors[2] = getDistanceSensor("ds2");
_dsensors[3] = getDistanceSensor("ds3");
_dsensors[4] = getDistanceSensor("ds4");
_dsensors[5] = getDistanceSensor("ds5");
_dsensors[6] = getDistanceSensor("ds6");
_dsensors[7] = getDistanceSensor("ds7");
_dsensors[8] = getDistanceSensor("ds8");
_dsensors[9] = getDistanceSensor("ds9");
_dsensors[10] = getDistanceSensor("ds10");
_dsensors[11] = getDistanceSensor("ds11");
_dsensors[12] = getDistanceSensor("ds12");
_dsensors[13] = getDistanceSensor("ds13");
_dsensors[14] = getDistanceSensor("ds14");
_dsensors[15] = getDistanceSensor("ds15");
// Enable all distance sensors
for (int i = 0; i < NUMBER_OF_DSENSORS; i++)
{
_dsensors[i]->enable(_time_step);
}
}
void Ant::drive(float distance) {
float speed = *travelVelocity;
float direction = getCompass();
util->tic(distance / speed * 1000);
move->forward(speed, speed);
//Drive while adjusting for detected objects and motor drift
while(1) {
driftCorrection(direction); //correct for motor drift
float distanceLeft = (util->timerStop - millis()) / 1000 * speed;
direction = collisionAvoidance(direction, distanceLeft); //check for collision; maneuver and update location
//If the timer has expired (i.e. we have reached our goal location)
if(util->isTime()) break;
}
move->stopMove();
}
void loop()
{
static uint32_t updelay;
const uint32_t curtime = millis();
if (updelay < curtime)
{
updelay = curtime + 100;
getCompass().read();
running_min.x = min(running_min.x, getCompass().m.x);
running_min.y = min(running_min.y, getCompass().m.y);
running_min.z = min(running_min.z, getCompass().m.z);
running_max.x = max(running_max.x, getCompass().m.x);
running_max.y = max(running_max.y, getCompass().m.y);
running_max.z = max(running_max.z, getCompass().m.z);
Serial.print("M min ");
Serial.print("X: ");
Serial.print((int)running_min.x);
Serial.print(" Y: ");
Serial.print((int)running_min.y);
Serial.print(" Z: ");
Serial.print((int)running_min.z);
Serial.print(" M max ");
Serial.print("X: ");
Serial.print((int)running_max.x);
Serial.print(" Y: ");
Serial.print((int)running_max.y);
Serial.print(" Z: ");
Serial.println((int)running_max.z);
}
rover5Task();
}
