Skip to content

Instantly share code, notes, and snippets.

/main.cpp Secret

Created Jun 16, 2016
Embed
What would you like to do?
#include <emc/io.h>
#include <emc/rate.h>
#include <iostream>
#include <algorithm> // std::min_element, std::max_element
#include <vector>
#include <cmath>
#include <sys/time.h>
#include "movement.h"
#include "detection.h"
#define PI 3.14159
int main()
{
double target_length_r = 0;
double target_length_l = 0;
double targetFlag = 2; //0 when initializing a targte, 1 when approaching a target, 2 when scanning for a new target
double turnFlag = 1;
Point potTarget, globalTarget;
double iniRot;
double direction = 2;
double extra_bit;
double pledgeCounter = 0;
Point straightTarget,leftTarget,rightTarget;
double pledgeFlag = 0;
double doorFlag = 0;
double before_door = 0;
struct timeval startTime;
int sit = 0;
int isdoor;
Edges edges;
// Create IO object, which will initialize the io layer
emc::IO io;
// Create Rate object, which will help using keeping the loop at a fixed frequency
emc::Rate r(10); // 10 Hz
// Initialize the movement
movement pico;
// Initialize the detection
detection detect;
// Loop while we are properly connected
while(io.ok())
{
emc::OdometryData odom;
emc::LaserData scan; //create an object in LaserData type to hold laser data
if (io.readLaserData(scan)) // if we get a new data
{
if ((isdoor==1)||(isdoor==2)||(isdoor==3)) { //check for a door
detect.doorCheck(io,isdoor,doorFlag,startTime,sit,scan,before_door);
}
else{ // when not checking for a door
if (targetFlag < 2){ // PICO is moving towards a target
pico.updateTarget(io, odom, globalTarget, potTarget, targetFlag, turnFlag, sit); // Relative target is updated based on global target
if (targetFlag < 2){ //There is not yet enough proximity to the target
pico.potFunction(io, scan, potTarget); // move to target
}
}
else{ // PICO is no longer moving towards a target
std::cout << "look for geometry, situeation = " << sit << std::endl;
potTarget.x = 1; // move forward
potTarget.y = 0; // do not move to the side
if (sit == 0){ // No geometry is detected (yet)
sit = detect.edgeDetection(io,scan,target_length_r,target_length_l,edges,isdoor,extra_bit,straightTarget,leftTarget,rightTarget); //detect situation
}
std::cout << "the situation is " << sit << std::endl;
if (sit == 0){ // After looking for geometry nothing has been detected
std::cout << "potential field" << std::endl;
pico.potFunction(io, scan, potTarget);
}
else{ // geometry is detected
if (pledgeFlag == 0){ // pledge has not yet been used to determine where to go
direction = detect.pledge(sit, pledgeCounter, targetFlag, pledgeFlag);
std::cout << "Situation " << sit << " Direction " << direction << " Counter " << pledgeCounter << std::endl;
}
std::cout << "direction " << direction << std::endl;
if (direction == 0){ //straight
pico.setTarget(io,odom,globalTarget,straightTarget.x,straightTarget.y,targetFlag, pledgeFlag); // make target the end of the junction
std::cout << "Target straight " << straightTarget.x << ", " << straightTarget.y << std::endl;
}
if (direction == 1){ //left
pico.setTarget(io,odom,globalTarget,leftTarget.x,leftTarget.y,targetFlag, pledgeFlag); // make target the end of the junction
std::cout << "Target left " << leftTarget.x << ", " << leftTarget.y << std::endl;
}
if (direction == 2){ //right
pico.setTarget(io,odom,globalTarget,rightTarget.x,rightTarget.y,targetFlag, pledgeFlag); // make target the end of the junction
std::cout << "Target right " << rightTarget.x << ", " << rightTarget.y << std::endl;
}
if (direction == 3){ //back
pico.rotate(io,odom, direction, turnFlag, pledgeFlag, iniRot, sit);
std::cout << "turn 180" << std::endl;
}
}
}
}
}
r.sleep();
}
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment