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