int main(void) {
struct snake *sn = construct_snake();
char *s;
printf("%s\n", s = toString(sn));
free(s);
goLeft(sn);
goLeft(sn);
goLeft(sn);
goUp(sn);
goUp(sn);
printf("%s\n", s = toString(sn));
free(s);
goRight(sn);
goRight(sn);
goDown(sn);
printf("%s\n", s = toString(sn));
free(s);
printf("ultima mossa eseguita: %d\n", goDown(sn));
destruct_snake(sn);
return 0;
}
//Task that controls the motor actions of the tank when it is
//set to autonomous mode
void vAutoMotor(void *vParameters) {
// avg0 ==> value sampled by the front sensor
// avg1 ==> value sampled by the back sensor
// avg2 ==> value sampled by the front-left sensor
// avg3 ==> value sampled by the front-right sensor
//true if the last move made by the motor was back,front,left or right
while(1) {
if (state == 5) { // state 5 represents the motor in autnomous mode
if (avg0 > 400) { //avg0 ==> the sensor in the front of the tank
//if the avg0 is less than 400 units, it beeps and indicates the
//tank to do one of the following actions:
if (avg1 > 400 && avg2 > 350 && avg3 > 350) {
//if all other sensors are close as well, go left
goLeft();
} else if (avg2 > 350 && avg3 > 350) {
//if both left and right side are open, then turn to the side
//farther away from an obstacle
if (avg2 > avg3) {
//right side of tank is closer to obstacle than left
goLeft();
} else { // this implies that avg2 < avg3
//left side of tank is closer to obstacle than right
goRight();
}
} else if (avg2 > 350) { //if the left side is open
goLeft();
} else if (avg3 > 350) { //if the right side is open
goRight();
} else {
if (avg2 > avg3) {
goLeft();
} else { // avg2 < avg3
goRight();
}
}
} else { // dist0 < 500
if (avg2 > 350 && avg3 > 350) {
if (avg2 > avg3) {
goLeft();
} else { // avg2 < avg3
goRight();
}
} else if (avg2 > 350) {
//if right sensor is close and left is clear
//then go upLeft
goUpLeft();
} else if (avg3 > 350) {
//if left sensor is close and right is clear
goUpRight();
} else { // By default if there is no obstruction
//the tank moves forward
goForward();
}
}
}
//delays time between task
vTaskDelay(10);
}
}
int Field::CheckJewel(Point ind, bool donotscore)
{
int s1=goLeft(ind);
s1+=goRight(ind)-1;
int s2=goUp(ind);
s2+=goDown(ind)-1;
int bs=0;
if (s2>2 && s1>2)
{
bs=s1+s2-1;
if (donotscore)
return bs;
At(ind)->LockUnlock();
goLeft(ind,true);
goRight(ind,true);
goUp(ind,true);
At(ind)->LockUnlock();
goDown(ind,true);
return bs;
}
if (s1>s2 && s1>2)
{
bs=s1-2;
if (donotscore)
return s1;
At(ind)->LockUnlock();
goLeft(ind,true);
At(ind)->LockUnlock();
goRight(ind,true);
return bs;
}
if (s2>s1 && s2>2)
{
bs=s2-2;
if (donotscore)
return s2;
At(ind)->LockUnlock();
goUp(ind,true);
At(ind)->LockUnlock();
goDown(ind,true);
return bs;
}
return bs;
}
task main()
{
wait1Msec(2000); //Robot waits for 2000 milliseconds before executing program
bMotorReflected[port2] = 1; //Reflects the direction of the motor on port2
goRight();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
goLeft();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
goStraight();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
goBack();
wait1Msec(10*1000); //Robot runs previous code for 10*1000 milliseconds before moving on
stopBot();
wait1Msec(1000);
} //Program ends, and the robot stops
int main()
{
CyGlobalIntEnable; /* Enable global interrupts. */
init();
/* Place your initialization/startup code here (e.g. MyInst_Start()) */
setSpeed(30000,MOTOR_A);
setSpeed(30000,MOTOR_B);
for(;;)
{
goForward();
CyDelay(2000);
stop_all();
CyDelay(2000);
goBackward();
CyDelay(2000);
stop_all();
CyDelay(2000);
goLeft();
CyDelay(2000);
stop_all();
CyDelay(2000);
goRight();
CyDelay(2000);
stop_all();
CyDelay(2000);
}
}
void RoboPult::Left()
{
if(askStena->isChecked () ){emit hasLeftWall(); askStena->setChecked(false);switchButt();return;};
if(askFree->isChecked () ){emit noLeftWall(); askFree->setChecked(false);switchButt();return;};
emit goLeft();
switchButt();
};
int CubesterRTP::getAction(Ogre::Vector3 playerPos)
{
int caseNum = (actionIndex/pathLength) % 2;
if(caseNum == 0 ) // 0 to pathLength go right
return goLeft();
else
reload();
}
void MotorCar::wriggle(uint8_t speed = SPEED_DEFAULT, uint8_t arc = ARC_DEFAULT) {
uint8_t r = 4;
while(r--) {
goLeft(speed, arc);
goRight(speed, arc);
}
}
void set_motion(int motion){
if (motion != curr_motion){
switch(motion){
case FORWARD: goForward(); break;
case LEFT: goLeft(); break;
case RIGHT: goRight(); break;
}
}
}
int AbstractCubester::move(int dir)
{
if(dir == 0 ) // 0 to pathLength go right
return goUp();
else if(dir == 1)
return goRight();
else if(dir == 2)
return goDown();
else if(dir == 3)
return goLeft();
}
void Mouse::move()
{
if (icon == 0)
goRight();
else if (icon == 1)
goLeft();
else if (icon == 2)
goUp();
else
goDown();
}
// Movement logic of the Create
// Preference is: Left, forward, right, backwards
void goToNextCell()
{
if(!leftWall && (getSomethingInTheWay() != LEFT))
goLeft();
else if(!frontWall && (getSomethingInTheWay() != FORWARD))
goForward();
else if(!rightWall && (getSomethingInTheWay() != RIGHT))
goRight();
else
goBackward();
}
int CubesterCCBLSquare::getAction(Ogre::Vector3 playerPos)
{
int caseNum = (actionIndex/pathLength) % 4;
if(caseNum == 0 ) // 0 to pathLength go right
return goRight();
else if(caseNum == 1)
return goUp();
else if(caseNum == 2)
return goLeft();
else if(caseNum == 3)
return goDown();
}
task main()
{
initializeRobot();
//waitForStart();
wait1Msec(delay);
goForward(2000);
spinLeft(500);
goForward(3000);
servoUp();
liftUp(3000);
drawerUp(4000);
spinLeft(1000);
goForward(3000);
goLeft(2000);
goForward(1000);
servoDown();
goBackward(1000);
goLeft(3000);
goForward(4000);
while (true){}
}
void main(void)
{
BYTE re1, re2;
BYTE a1, a2, a3, a4, a5, a6;
M48Init();
printf("\r\nM88PA demo\r\n");
pip();
#define T 1000
PORTB.1 = 1;
PORTB.2 = 1;
robotStop();
goFwd();
delay_ms(T);
goBack();
delay_ms(T);
goLeft();
delay_ms(T);
goRight();
delay_ms(T);
robotStop();
pip();
while (1)
{ //char c;
//c = getchar();
//printf("[%c] %3d\r\n",c, (int)c);
re1 = ReadByteADC(ADC_E1);
re2 = ReadByteADC(ADC_E2);
a1 = ReadByteADC(ADC_1);
a2 = ReadByteADC(ADC_2);
a3 = ReadByteADC(ADC_3);
a4 = ReadByteADC(ADC_4);
a5 = ReadByteADC(ADC_5);
a6 = ReadByteADC(ADC_6);
delay_ms(20);
printf("%4u %4u %4u %4u %4u %4u %4u %4u\r", re1, re2, a1, a2, a3, a4, a5, a6);
}
}
void Npc::updateCurrent(CommandQueue &command_queue, const sf::Time dt)
{
if (isActive())
{
updateAnimation(dt);
action_queue.update(dt);
if (getCurrentDirection() == Direction::Left) goLeft(dt);
else if (getCurrentDirection() == Direction::Right) goRight(dt);
else if (getCurrentDirection() == Direction::Up) goUp(dt);
else if (getCurrentDirection() == Direction::Down) goDown(dt);
}
}
int Field::goLeft(Point ind, bool scoreit)
{
Point next_ind = ind;
next_ind.x-=1;
int sum=1;
if (At(next_ind) && At(next_ind)->IsID(At(ind))) sum+=goLeft(next_ind,scoreit);
if (scoreit)
{
StartAnimation(ind);
}
return sum;
}
void Enemy::patrol()
{
std::discrete_distribution<int> dir_distribution({ 60, 10, 10, 10, 10 });
std::uniform_int_distribution<int> distance_distribution(50, 100);
std::uniform_real_distribution<float> wait_time_distribution(1.f, 3.f);
Direction dir = static_cast<Direction>(dir_distribution(mt));
int distance = distance_distribution(mt);
stop_time = sf::seconds(wait_time_distribution(mt));
//Action selection
Action patrol;
patrol.thread_id = 1;
patrol.action = get_action([this, dir, distance](sf::Time dt)
{
if (dir == Direction::Left) { moveLeft(distance); }
else if (dir == Direction::Right) { moveRight(distance); }
else if (dir == Direction::Up) { moveUp(distance); }
else if (dir == Direction::Down) { moveDown(distance); }
//else if (dir == Direction::None) { std::cout << "Stop time" << stop_time.asSeconds() << std::endl; wait(stop_time); }
return false;
});
//When we have 2 actions we stop action addition
unsigned int max_actions = 2;
if (action_tree.threadSize(patrol.thread_id) < max_actions)
{
pushAction(patrol);
}
//Action updating, each frame
Action patrol_update;
patrol_update.thread_id = 0;
patrol_update.action = get_action([this](sf::Time dt)
{
if (getCurrentDirection() == Direction::Left) { goLeft(dt); }
else if (getCurrentDirection() == Direction::Right) { goRight(dt); }
else if (getCurrentDirection() == Direction::Up) { goUp(dt); }
else if (getCurrentDirection() == Direction::Down) { goDown(dt); }
return false;
});
pushAction(patrol_update);
}
void updateController(float dt)
{
action_add_subtype(move_act, SACT_DISABLED);
if (keysDown & leftKey)
goLeft(dt);
if (keysDown & rightKey)
goRight(dt);
if (keysDown & aKey)
goLeft(dt*0.2f);
if (keysDown & dKey)
goRight(dt*0.2f);
if (keysDown & downKey)
{
action_remove_subtype(move_act, SACT_DISABLED);
move_angle->fvalue = M_PI;
//goDown(dt);
}else
if (keysDown & upKey)
{
action_remove_subtype(move_act, SACT_DISABLED);
move_angle->fvalue = 0;
//goUp(dt);
}
}
//vTaskContolMotor is a Task that controls the movements of the motor
//the motor can move right, left, forward, backward, upRight, upLeft,
//downRight and downLeft
//PD7 ==> AOUT
//PD3 ==> AIN
//PD4 ==> BOUT
//PD2 ==> BIN
//PD6 ==> STDBY
void vTaskControlMotor(void *vParameters) {
//the following commands for the controlling the tank through
// the blue tooth app "blueTerm"
//'w' pressed to go forward
//'s' pressed to go back
//'a' pressed to go left
//'d' pressed to go right
//'q' pressed to go upLeft
//'e' pressed to go upRight
//'z' pressed to go back left
//'x' pressed to go back right
//if any other character is pressed, the tank stops
while(1) {
//gets the currentKey pressed by the user
currentKey = keymaster();
if (state == 6) { // (!auton) {
//if the up key is pressed, tank should move forward
if (currentKey == 1 || bluetoothSignal == 'w') {
goForward();
} else if (currentKey == 2 || bluetoothSignal == 's') {//if down is pressed
goBackWard();
} else if (currentKey == 3 || bluetoothSignal == 'a') { //if left is pressed
goLeft();
} else if (currentKey == 4 || bluetoothSignal == 'd') { //if right is pressed
goRight();
} else if(currentKey == 6 || bluetoothSignal == 'q'){ // if the upLeft key is pressed
goUpLeft();
} else if(currentKey == 7 || bluetoothSignal == 'e'){ // if the upRight key is pressed
goUpRight();
} else if(currentKey == 8 || bluetoothSignal == 'z'){ //if the backleft key is pressed
goBackLeft();
} else if(currentKey == 9 || bluetoothSignal == 'x'){//if the backRight key is pressed
goBackRight();
} else {
stopTank();
}
}
if (bluetoothSignal == 'r') {
state = 2;
}
//delays time between task
vTaskDelay(20);
}
}
/**
* Make move.
* @return move symbol or SYM_NONE for bad move
*/
char
Unit::driveOrder(char move)
{
if (canDrive()) {
if (m_symbols.getLeft() == move) {
return goLeft();
}
if (m_symbols.getRight() == move) {
return goRight();
}
if (m_symbols.getUp() == move) {
return goUp();
}
if (m_symbols.getDown() == move) {
return goDown();
}
}
return ControlSym::SYM_NONE;
}
/**
* Test keys and try move, use borrowed controls.
* @return a symbol when unit has moved or SYM_NONE
*/
char
Unit::driveBorrowed(const InputProvider *input, const KeyControl &buttons)
{
if (canDrive()) {
if (input->isPressed(buttons.getLeft())) {
return goLeft();
}
if (input->isPressed(buttons.getRight())) {
return goRight();
}
if (input->isPressed(buttons.getUp())) {
return goUp();
}
if (input->isPressed(buttons.getDown())) {
return goDown();
}
}
return ControlSym::SYM_NONE;
}
/*
* This controls where the cursor moves to next.
* Press w to go up
* Press a to go left
* Press s to go down
* Press d to go right
*/
void control(unsigned char c, point * p, int (*board)[width]){
if(c == 'w'){
goUp(p, board);
}
if(c == 'a'){
goLeft(p, board);
}
if(c == 's'){
goDown(p,board);
}
if(c == 'd'){
goRight(p,board);
}
//9 corresponds to Tab
if(c == 9){
clearGrid(board, p);
}
}
task main(){
goForward(25);
wait1Msec(500);
goBackward(25);
wait1Msec(500);
goLeft(25);
wait1Msec(500);
goRight(25);
wait1Msec(500);
LeftForward(25);
wait1Msec(500);
LeftBackward(25);
wait1Msec(500);
RightForward(25);
wait1Msec(500);
RightBackward(25);
wait1Msec(500);
halt();
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent)
{
qDebug("Test");
setupUi(this);
bt = new Bluetooth("00:16:53:03:9D:7E");
if (!bt->getNxtSocket()) {
statusbar->showMessage("Fehler");
}
else {
statusbar->showMessage(QString("connected %1").arg(bt->getNxtSocket()));
}
update = new QTimer(this);
update->setInterval(1000);
connect(update, SIGNAL(timeout()), this, SLOT(updateSensors()));
update->start();
motorLeft = new Motor(bt, OUT_C);
motorRight = new Motor(bt, OUT_B);
motorLeft->setSync();
motorRight->setSync();
motorThrottle->setMaximum(100);
connect(motorThrottle, SIGNAL(valueChanged(int)), throttleDisplay,
SLOT(setNum(int)));
sonar = new Ultrasonic(bt, IN_4);
light = new Light(bt, IN_3);
connect(buttonUp, SIGNAL(pressed()), this, SLOT(goForward()));
connect(buttonUp, SIGNAL(released()), this, SLOT(stop()));
connect(buttonDown, SIGNAL(pressed()), this, SLOT(goBackward()));
connect(buttonDown, SIGNAL(released()), this, SLOT(stop()));
connect(buttonLeft, SIGNAL(pressed()), this, SLOT(goLeft()));
connect(buttonLeft, SIGNAL(released()), this, SLOT(stop()));
connect(buttonRight, SIGNAL(pressed()), this, SLOT(goRight()));
connect(buttonRight, SIGNAL(released()), this, SLOT(stop()));
}
void DebugProc(void)
{
BYTE re1, re2;
BYTE a1, a2, a3, a4, a5, a6;
printf("\r\nTEST REGIME\r\n");
// Отладочные телодвижения
// (проверка правильности подключения двигателей: вперед, назад, влево, вправо)
#define Time 500
goFwd(); delay_ms(Time);
goBack(); delay_ms(Time);
goLeft(); delay_ms(Time);
goRight(); delay_ms(Time);
robotStop();
pip();
goFastLeft(); delay_ms(Time);
goFastRight(); delay_ms(Time);
robotStop();
pip();
while (1)
{
re1 = ReadByteADC(ADC_E1);
re2 = ReadByteADC(ADC_E2);
a1 = ReadByteADC(ADC_1);
a2 = ReadByteADC(ADC_2);
a3 = ReadByteADC(ADC_3);
a4 = ReadByteADC(ADC_4);
a5 = ReadByteADC(ADC_5);
a6 = ReadByteADC(ADC_6);
delay_ms(20);
printf("(%4u %4u) (%4u %4u) %4u %4u %4u %4u\r", re1, re2, a1, a2, a3, a4, a5, a6);
}
}
//this task is responsible for controlling tank actions in semi-autonomous mode
//if the tank is close to an obstacle it will being moving in the oposite direction to
//avoid collisions.
void vSemiMotor(void *vParameters) {
while(1) {
if (state == 9) { // (!auton) {
//if the up key is pressed, tank should move forward
if (avg0 > 400 || avg1 > 400 || avg2 > 400 || avg3 > 400) {
if (avg1 > 400) {
if (avg0 > 400) {
if (avg3 > 400 && avg2 > 400) {
goRight(); // tentative
} else if (avg3 > 400) {
goRight();
} else if (avg2 > 400) {
goLeft();
} else {
goLeft(); //tentative
}
} else { // avg0 <= 400
if (avg3 > 400 && avg2 > 400) {
goRight();
} else if (avg3 > 400) {
goUpRight();
} else if (avg2 > 400) {
goUpRight();
} else {
goForward();
}
}
} else { // avg1 <= 400
if (avg0 > 400) {
if (avg3 > 400 && avg2 > 400) {
goRight(); // tentative
} else if (avg3 > 400) {
goRight();
} else if (avg2 > 400) {
goLeft();
} else {
goRight(); // tentative
}
} else { // avg0 <= 500
if (avg3 > 400 && avg2 > 400) {
goRight(); // tentative
} else if (avg3 > 400) {
goUpRight();
} else { // avg2 > 500
goUpLeft();
}
}
}
} else {
if (currentKey == 1 || bluetoothSignal == 'w') {
goForward();
} else if (currentKey == 2 || bluetoothSignal == 's') {//if down is pressed
goBackWard();
} else if (currentKey == 3 || bluetoothSignal == 'a') { //if left is pressed
goLeft();
} else if (currentKey == 4 || bluetoothSignal == 'd') { //if right is pressed
goRight();
} else if(currentKey == 6 || bluetoothSignal == 'q'){ // if the upLeft key is pressed
goUpLeft();
} else if(currentKey == 7 || bluetoothSignal == 'e'){ // if the upRight key is pressed
goUpRight();
} else if(currentKey == 8 || bluetoothSignal == 'z'){ //if the backleft key is pressed
goBackLeft();
} else if(currentKey == 9 || bluetoothSignal == 'x'){//if the backRight key is pressed
goBackRight();
} else {
stopTank();
}
}
}
//delays time between task
vTaskDelay(20);
}
}
geometry_msgs::PoseStamped getNextWaypoint(const sensor_msgs::PointCloud& pointCloud){
geometry_msgs::PoseStamped lcl_position;//our next waypoint in navigating from our initial position to the global goal position.
bool left_occupied = false, right_occupied = false, front_occupied = false, self_occupied = false, left_front_occupied = false, right_front_occupied = false;//occupancy grid Booleans.
float currPoseX = current_position.pose.position.x;//absolute x-position of laser.
float currPoseY = current_position.pose.position.y;//aboslute y-position of laser.
float currPoseYaw = 2*asin(current_position.pose.orientation.z); //transform absolute laser yaw from quaternion to radians
float currPointXabs, currPointYabs;//absolute x and y position of current point in pointcloud.
timestamp = current_position.header.stamp.sec;
//(START A)------------------------------
// Determine whether adjacent block to left, right, right-front, left-front, and front of laser are occupied. Change occupancy grid size with "navigation_resolution_" global variable.
for(int i = 0; i<pointCloud.points.size(); i++){//iterate through all points in pointcloud.
const geometry_msgs::Point32& currPoint(pointCloud.points[i]);//get current point in pointcloud.
//calculate absolute position of current point by offsetting it relative to the absolute position of the laser.
currPointXabs = currPoint.x + currPoseX;
currPointYabs = currPoint.y + currPoseY;
int j = pointCloud.points.size();
const geometry_msgs::Point32& right(pointCloud.points[0]);
const geometry_msgs::Point32& front(pointCloud.points[j/2]);
const geometry_msgs::Point32& left(pointCloud.points[j-5]);
rcl = -right.y;
fcl = front.x;
lcl = left.y;
clearance.pose.position.x = rcl;
clearance.pose.position.y = fcl;
clearance.pose.position.z = lcl;
pub_clearance.publish(clearance);
//determine our occupancy grid relative to the laser based on Cartesian pointcloud data. The coefficient in front of "navigation_resolution_" sets the bounds of how much of the block to check in. E.g. "1.5*navigation_resolution_" is "one and a half occupancy grid blocks".
if(currPointYabs < (currPoseY + gcupper*navigation_resolution_y) && currPointYabs > (currPoseY + gclower*navigation_resolution_y) && currPointXabs > (currPoseX - gclower*navigation_resolution_x) && currPointXabs < (currPoseX + gclower*navigation_resolution_x)){
left_occupied = true;
}
if(currPointYabs < (currPoseY + gcupper*navigation_resolution_y) && currPointYabs > (currPoseY + gclower*navigation_resolution_y) && currPointXabs > (currPoseX + gclower*navigation_resolution_x) && currPointXabs < (currPoseX + gcupper*navigation_resolution_x)){
left_front_occupied = true;
}
if(currPointYabs < (currPoseY - gclower*navigation_resolution_y) && currPointYabs > (currPoseY - gcupper*navigation_resolution_y) && currPointXabs > (currPoseX - gclower*navigation_resolution_x) && currPointXabs < (currPoseX + gclower*navigation_resolution_x)){
right_occupied = true;
}
if(currPointYabs < (currPoseY - gclower*navigation_resolution_y) && currPointYabs > (currPoseY - gcupper*navigation_resolution_y) && currPointXabs > (currPoseX + gclower*navigation_resolution_x) && currPointXabs < (currPoseX + gcupper*navigation_resolution_x)){
right_front_occupied = true;
}
if(currPointYabs < (currPoseY + gclower*navigation_resolution_y) && currPointYabs > (currPoseY - gclower*navigation_resolution_y) && currPointXabs > (currPoseX + gclower*navigation_resolution_x) && currPointXabs < (currPoseX + gcupper*navigation_resolution_x)){
front_occupied = true;
}
if(currPointYabs < (currPoseY + gclower*navigation_resolution_y) && currPointYabs > (currPoseY - gclower*navigation_resolution_y) && currPointXabs > (currPoseX - gclower*navigation_resolution_x) && currPointXabs < (currPoseX + gclower*navigation_resolution_x)){
self_occupied = true;
}
}
//DEBUG. Use this block to check how previous for-loop is building the local occupancy grid.
std_msgs::String msg;
std::stringstream ss;
// ss << "left: " << left_occupied;
// ss << ", left-front: " << left_front_occupied;
// ss << ", right: " << right_occupied;
// ss << ", right-front: " << right_front_occupied;
// ss << ", front: " << front_occupied;
// ss << ", self: " << self_occupied;
// ss << ", CurrYaw: " << currPoseYaw*(180/3.14);
//\DEBUG
//(END A)------------------------------
//(START B)------------------------------
// Determine angular bearing from current position to global goal position.
double global_goal_bearing = getGlobalGoalBearing(currPoseX, currPoseY);
//(END B)--------------------------------
/*
//(Algorithm LHS)-------------------------------------------------------------------------------LEFT WALL FOLLOWING BEGINS---------------------------------------------------------------------------------------------
if (flagTR == 0)
{
if ((currPoseY < leftgo) && (flag1 == 0)){
//go left
ss << ", go left";
lcl_position = goLeft(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
//ss << ", YawDot: ";
//ss << yawdot*(180/3.14);
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if (currPoseY >= leftgo)
{
flag1 = 1;
}
if ((!front_occupied) && (flag1 == 1) && (flag2 == 0) && (flag3 == 0))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
//ss << ", YawDot: ";
//ss << yawdot*(180/3.14);
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((front_occupied) && (flag1 == 1))
{
flag2 = 1;
}
if ((flag1 == 1) && (flag2 == 1) && (currPoseY > 0.9)){
//go right
ss << ", go right";
lcl_position = goRight(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
//ss << ", YawDot: ";
//ss << yawdot*(180/3.14);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((!front_occupied) && (flag1 == 1) && (flag2 == 1))
{
flag3 = 1;
}
if ((flag1 == 1) && (flag2 == 1) && (flag3 == 1) && (currPoseY > -0.9) && (currPoseY < 0.9))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
//ss << ", YawDot: ";
//ss << yawdot*(180/3.14);
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if (currPoseX >= tr)
{//reached target room
flagTR = 1;
}
}
//(END Algorithm LHS)---------------------------------------------------------------------------------LEFT WALL FOLLOWING ENDS----------------------------------------------------------------------------------------------------
*/
//(Algorithm RHS)-------------------------------------------------------------------------------RIGHT WALL FOLLOWING BEGINS---------------------------------------------------------------------------------------------
if (flagTR == 0)
{
if ((rcl > 1.8) && (flag1 == 0)){
//go right
ss << ", go right";
lcl_position = goRight(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if (rcl <= 1.8)
{
flag1 = 1;
}
if ((!front_occupied) && (flag1 == 1) && (flag2 == 0) && (flag3 == 0))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((front_occupied) && (flag1 == 1))
{
flag2 = 1;
}
if ((flag1 == 1) && (flag2 == 1) && (currPoseY < openingYlow+0.3)){
//go left
ss << ", go left";
lcl_position = goLeft(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((!front_occupied) && (flag1 == 1) && (flag2 == 1) && (currPoseY >= openingYlow+0.3))
{
flag3 = 1;
}
if (((!front_occupied) && (flag1 == 1) && (flag2 == 1) && (flag3 == 1)) && (currPoseY < openingYhigh-0.3))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if (currPoseX >= tr)
{
//reached target room
flagTR = 1;
}
}
//(END Algorithm RHS)-------------------------------------------------------------------------------RIGHT WALL FOLLOWING ENDS-------------------------------------------------------------------------------------------
/*
//(Algorithm FRONT)-------------------------------------------------------------------------------STRAIGHT FORWARD BEGINS---------------------------------------------------------------------------------------------
if (flagTR == 0)
{
if ((!front_occupied) && (flag1 == 0) && (flag2 == 0) && (flag3 == 0))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if (front_occupied)
{
flag1 = 1;
}
if ((flag1 == 1) && (flag2 == 0) && (flag3 == 0) && (currPoseY > -(brc+1.5)))
{
//go right
ss << ", go right";
lcl_position = goRight(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((currPoseY <= -(brc+1.5)) && (!front_occupied) && (flag1 == 1))
{
flag2 = 1;
}
if ((currPoseY <= -(brc+1.5)) && (flag1 == 1) && (flag2 == 1) && (flag3 == 0))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((front_occupied) && (flag1 == 1) && (flag2 == 1))
{
flag3 = 1;
}
if ((flag1 == 1) && (flag2 == 1) && (flag3 == 1) && (currPoseY < -0.9)){
//go left
ss << ", go left";
lcl_position = goLeft(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if ((!front_occupied) && (flag1 == 1) && (flag2 == 1) && (flag3 == 1) && (currPoseY >= -0.9) && (currPoseY <= 0.9))
{
//go forward
ss << ", go forward";
lcl_position = goForward(currPoseX, currPoseY, currPoseYaw);
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
if (currPoseX >= tr)
{//reached target room
flagTR = 1;
}
}
//(END Algorithm FRONT)-------------------------------------------------------------------------------STRAIGHT FORWARD ENDS-------------------------------------------------------------------------------------------
*/
// (SWITCH TO TARGET ESTIMATION)
if (flagTR == 1)
{
lcl_position.pose.position.x = nxtX;
lcl_position.pose.position.y = nxtY;
lcl_position.pose.orientation.z = nxtYaw;
xdot = (currPoseX - x_prev)/dt;
ydot = (currPoseY - y_prev)/dt;
yawdot = (currPoseYaw - yaw_prev)/dt;
wp_x_dot = (lcl_position.pose.position.x - last_wp_x)/dt;
wp_y_dot = (lcl_position.pose.position.y - last_wp_y)/dt;
wp_yaw_dot = (lcl_position.pose.orientation.z - last_wp_yaw)/dt;
if (wp_x_dot > xddlim)
{
wp_x_dot = xddlim;
}
else if (wp_x_dot < -xddlim)
{
wp_x_dot = -xddlim;
}
if (wp_y_dot > yddlim)
{
wp_y_dot = yddlim;
}
else if (wp_y_dot < -yddlim)
{
wp_y_dot = -yddlim;
}
th_des = kp_theta*(lcl_position.pose.position.y - currPoseY) + kd_theta*(wp_y_dot - ydot); // roll
phi_des = kp_phi*(lcl_position.pose.position.x - currPoseX) + kd_phi*(wp_x_dot - xdot); // pitch
yaw_des = kp_psi*(lcl_position.pose.orientation.z - currPoseYaw) + kd_psi*(wp_yaw_dot - yawdot); // yaw
if (phi_des > 10)
{
phi_des = 10;
}
else if (phi_des < -10)
{
phi_des = -10;
}
if (th_des > 10)
{
th_des = 10;
}
else if (th_des < -10)
{
th_des = -10;
}
if (yaw_des > 15)
{
yaw_des = 15;
}
else if (yaw_des < -15)
{
yaw_des = -15;
}
del_phi = 62 - phi_des;
del_th = 70 - th_des;
del_yaw = 64 - yaw_des;
ss << " Roll: ";
ss << del_th;
ss << ", Pitch: ";
ss << del_phi;
ss << ", Yaw: ";
ss << del_yaw;
address_p = 0x84;
value_p = int (del_phi);
send_cmd(address_p,value_p);
address_r = 0x85;
value_r = int (del_th);
send_cmd(address_r,value_r);
address_y = 0x83;
value_y = int (del_yaw);
send_cmd(address_y,value_y);
}
//DEBUG
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
//\DEBUG
x_prev = currPoseX;
y_prev = currPoseY;
yaw_prev = currPoseYaw;
last_wp_x = lcl_position.pose.position.x;
last_wp_y = lcl_position.pose.position.y;
last_wp_yaw = lcl_position.pose.orientation.z;
return lcl_position;
}
void main(void)
{
int T; // Счетчик тактов для смены тактики поиска
int a; // Куда крутиться, 0 - влево, 1 - вправо, 2 - вперёд
BYTE cntOtval = 0; // Счетчик числа импульсов для опускания отвала
// Инициализация контроллера
M48Init();
// Port C initialization
// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State6=T State5=P State4=P State3=P State2=P State1=T State0=T
PORTC=0x3C;
DDRC=0x00;
// Переопределяем ef_2 (D.4) как цифровой подтянутый вход
// Port D initialization
// Func7=Out Func6=Out Func5=Out Func4=In Func3=Out Func2=Out Func1=In Func0=In
// State7=0 State6=0 State5=0 State4=P State3=0 State2=0 State1=T State0=T
PORTD=0x10;
DDRD=0xEC;
// Скроостью управлять не будем. Выставляем по максиммуму
PORTB.1 = 1;
PORTB.2 = 1;
robotStop();
pip();
printf("\r\nSumo 88 RK-25/26MS Version 1.05\r\n\r\n");
//--------------------------------------------------------
// Переход в отладочный режим
// Для перехода в отладочный режим необходимо, чтоб перед включением датчики
// границы находились на светлом фоне
//--------------------------------------------------------
FotoL = ReadByteADC(6)>LimLeft;
FotoR = ReadByteADC(7)>LimRight;
if(FotoL && FotoR)
DebugProc();
robotStop();
//--------------------------------------------------------
// Ожидание сигнала START
//--------------------------------------------------------
while(SENSOR_START==0);
/***
// Выдача управляющах импульсов на сервомашинку, для опускания отвала
// Изображаем управляющий ШИМ
// Это займет около 10*(18+0.9) = 189 мс.
for(n=0;n<MAX_CNT_OTVAL;n++)
{
ef_1 = 0;
delay_us(18000); // Период импульсов. Вообще должно быть так: 20000 - <ширина импульса> = 20000-900 = 19100
ef_1 = 1;
delay_us(900); // Ширина импульса 900 мкс (0.9 мс) - крайнее положение
}
***/
ef_1 = 0;
T = 0;
a = 0;
//--------------------------------------------------------
// Основной цикл
//--------------------------------------------------------
while (1)
{
//------------------------------------------------------
// Опускаем отвал на ходу
//------------------------------------------------------
if(cntOtval<MAX_CNT_OTVAL)
{
cntOtval++;
//Выдаем импульс (с задержками)
delay_us(18000); // Период импульсов. Вообще должно быть так: 20000 - <ширина импульса> = 20000-900 = 19100
ef_1 = 1;
delay_us(900); // Ширина импульса 900 мкс (0.9 мс) - крайнее положение
ef_1 = 0;
}
// Реакция на сигнал СТОП
if(SENSOR_START==0)
{
robotStop();
pip();
while(1);
}
// Считываем сигналы датчиков
SharpSL = (sen_3==0);
SharpSR = (sen_4==0);
SharpFL = (sen_5==0);
SharpFR = (sen_6==0);
FotoL = ReadByteADC(6)>LimLeft;
FotoR = ReadByteADC(7)>LimRight;
//------------------------------------------------------
// Безусловные рефлексы
// Сначала и идет обработка самых приоритетных сигналов
//------------------------------------------------------
// Реакция на границу поля
if(FotoL)
{
// Отъезжаем назад
goBack();
delay_ms(100);
// Начинаем крутиться
goRight();
T = 0;
continue;
}
if(FotoR)
{
// Отъезжаем назад
goBack();
delay_ms(100);
// Начинаем крутиться
goLeft();
T = 0;
continue;
}
//------------------------------------------------------
// Общие действия
// Обнаружение противника передними датчиками
//------------------------------------------------------
if(SharpFL&&SharpFR)
{
goFwd();
T = 0;
continue;
}
if(SharpFL||SharpSL)
{
goLeft();
T = 0;
a = 0;
continue;
}
if(SharpFR||SharpSR)
{
goRight();
T = 0;
a = 1;
continue;
}
//------------------------------------------------------
// Никого не обнаружили
// Поиск противника
//------------------------------------------------------
if(a==0) goLeft();
if(a==1) goRight();
if(a==2) goFwd();
T=T+1;
if(T>MAXT) // Пора менять тактику
{
//pip();
T=0;
// Выбираем действие "случайным" образом
a=rand()%3; // Остаток от деления на 3 (деление по модулю 3)
}
}
}
void move(Cell maze[][16],int *mx,int *my,int *dir) {
if (goodFront(maze,*mx,*my,*dir)) go(mx,my,*dir);
else if (goodRight(maze,*mx,*my,*dir)) goRight(mx,my,dir);
else if (goodLeft(maze,*mx,*my,*dir)) goLeft(mx,my,dir);
else goBack(mx,my,dir);
}
