void DriveMode::handleController(std::vector<int> previousButtons, std::vector<float> previousAxes, const sensor_msgs::Joy &joy)
{
ControllerMode::handleController(previousButtons, previousAxes, joy);
// speed of the base
if (joy.axes[PS3_AXIS_LEFT_HORIZONTAL] || joy.axes[PS3_AXIS_RIGHT_VERTICAL]) // we are sending speed
sendSpeed(joy.axes[PS3_AXIS_RIGHT_VERTICAL], joy.axes[PS3_AXIS_LEFT_HORIZONTAL]);
else if (previousAxes[PS3_AXIS_LEFT_HORIZONTAL] || previousAxes[PS3_AXIS_RIGHT_VERTICAL]) // we stopped sending speed
sendSpeed(0.f, 0.f);
// arm
if (pressed(previousButtons, joy, PS3_R1))
sendArmPosition(ARM_POS_UP_X, ARM_POS_UP_Z);
if (pressed(previousButtons, joy, PS3_R2))
sendArmPosition(ARM_POS_DOWN_X, ARM_POS_DOWN_Z);
// gripper
if (pressed(previousButtons, joy, PS3_L1))
sendGripperState(true);
if (pressed(previousButtons, joy, PS3_L2))
sendGripperState(false);
// ping sensor
if (pressed(previousButtons, joy, PS3_SELECT))
{
mPingState = !mPingState;
sendPingState(mPingState);
}
}
void home() {
if (current > 1) {
sendSpeed(0.0, 0.0);
return;
}
// If leaving, go forward and turn
if (direction == 1) {
straightBlind(5.5 * 12);
int chk = turn90('R');
//printf("Turn returned %d\n", chk);
currentRoom = -1;
current++;
return;
}
// If returning, turn and move forward
if (direction == -1) {
int chk = turn90('L');
straightBlind(4.5 * 12);
currentRoom = 0;
current = 0;
return;
}
}
void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
if(!moving && (std::abs(joy->axes[axisindex[0]])>0.05||std::abs(joy->axes[axisindex[1]])>0.05||std::abs(joy->axes[axisindex[2]])>0.05)){
desired_bMe=bMe;
desired_bMe[0][3]-=0.3*joy->axes[axisindex[0]]*AxisDir[0];
desired_bMe[1][3]-=0.3*joy->axes[axisindex[1]]*AxisDir[1];
desired_bMe[2][3]-=0.3*joy->axes[axisindex[2]]*AxisDir[2];
//std::cerr<<"Desired bMe"<<std::endl<<desired_bMe<<std::endl;
next_joints=robot->armIK(desired_bMe);
//std::cerr<<"Desired joints"<<std::endl<<next_joints<<std::endl;
//If valid joints and reasonable new position ... ask to MOVE
if(next_joints[0]>-1.57 && next_joints[0]<2.1195 && next_joints[1]>0 && next_joints[1]<1.58665 && next_joints[2]>0 && next_joints[2]<2.15294){
if(std::abs(desired_bMe[0][3]-bMe[0][3])<5 && std::abs(desired_bMe[1][3]-bMe[1][3])<5 && std::abs(bMe[2][3]-bMe[2][3])<5)
moving=true;
else
ROS_INFO("Error: New position too far form the original position.");
}else
ROS_INFO("Error: Unreachable position.");
}
//Save Jaw movements for later use.
q3=joy->axes[axisindex[3]];
q4=joy->axes[axisindex[4]];
//TODO Send speed: to avoid depending on joystick callbacks could be called from
//a while loop on main program setting desired rate. Current version doesn't sendSpeed if
//if /joy topic is silent.
sendSpeed();
}
void DriveMode::onDeactivate()
{
ROS_INFO("[DriveMode] Deactivated.");
sendSpeed(0.f, 0.f);
mActive = false;
}
void control(int lostL, double lL, double aL, double bL, double *endPointsL, int lostR, double lR, double aR, double bR, double *endPointsR, double minDistance, double walling) {
const float maxSpeed = 100.0; // Maximum PWM duty cycle
const float sensitivity = 0.015; // Sensitivity of errors in correcting the wheels (higher is more drastic changes in wheel speed)
printf("lostL: %d lostR: %d\n", lostL, lostR);
const double minLength = 150.0;
// Obstacle!
if (minDistance < 200.0) {
sendSpeed(0.0, 0.0);
return;
}
double wallDistL = aL / sqrt(bL * bL + 1);
double wallDistR = aR / sqrt(bR * bR + 1);
// Both walls detected and two walling requested
if (!lostL && !lostR && (walling == 0.0) && lR > minLength && lL > minLength) {
double diff = wallDistR + wallDistL;
float adjust = maxSpeed - (fabs(diff) * sensitivity);
if (adjust < 0) adjust = 0;
char buffer[50];
if (diff < 0) {
sendSpeed(maxSpeed, adjust);
} else {
// printf("L: %f R: %f\n", adjust, maxSpeed);
sendSpeed(adjust, maxSpeed);
}
diff = previous_diff;
}
// One wall detected and one walling requested
else if ((!lostL || !lostR) && (walling != 0.0)) {
double diff;
float adjust;
char buffer[50];
if (walling > 0 && !lostL && lL > minLength) {
diff = aL - walling;
adjust = maxSpeed - (fabs(diff) * sensitivity);
if (adjust < 0) adjust = 0;
if (diff > 0) {
sendSpeed(adjust, maxSpeed);
} else {
sendSpeed(maxSpeed, adjust);
}
}
else if (!lostR && lR > minLength) {
diff = aR - walling;
adjust = maxSpeed - (fabs(diff) * sensitivity);
if (adjust < 0) adjust = 0;
if (diff > 0) {
sendSpeed(maxSpeed, adjust);
} else {
sendSpeed(adjust, maxSpeed);
}
}
}
}
void DriveMode::sendSpeedEvent()
{
ROS_INFO("Speed Event.");
if (mActive)
{
mTimer.expires_at(mTimer.expires_at() + boost::posix_time::seconds(1));
mTimer.async_wait(boost::bind(&DriveMode::sendSpeedEvent, this));
sendSpeed(mLastLinearSpeed, mLastAngularSpeed);
}
}
void Bt_command(void) {
if (UartFlags.rewrite) {
unsigned char tmp = 0;
switch (BT_command[tmp++]) {
case 'S':
switch (BT_command[tmp++]) {
case 'T':
RtcTimeWDay = ascToTime(&BT_command[tmp]);
break;
case 'W':
wupTime = ascToTime(&BT_command[tmp]);
TimeFlags.wakeup = 1;
break;
case 'P':
sleepTime = ascToTime(&BT_command[tmp]);
TimeFlags.gosleep = 1;
break;
case 'D':
uiNastawa = ascToSpeed(&BT_command[tmp]);
break;
case 'F':
if (BT_command[tmp++] == 'P') TimeFlags.gosleep = BT_command[tmp++] - '0';
if (BT_command[tmp++] == 'W') TimeFlags.wakeup = BT_command[tmp] - '0';
break;
default:
break;
} break;
case 'G':
switch (BT_command[tmp]){
case 'D':
sendSpeed(uiNastawa);
break;
case 'F':
sendFlags(&TimeFlags);
break;
case 'P':
sendTime(&sleepTime,'P');
break;
case 'T':
sendTime(&RtcTimeWDay, 'T');
break;
case 'W':
sendTime(&wupTime,'W');
break;
default:
break;
}
default:
break;
}
UartFlags.rewrite = 0;
}
}
void DCC_Proxy::update(void)
{
if(!isPermitted())
return;
//check to see if we need to emit a producer identified event
if (_producer_identified_flag)
{
OLCB_Event e(PROXY_SLOT_OCCUPIED);
while (!_link->sendProducerIdentified(NID, &e));
_producer_identified_flag = false;
}
uint32_t new_time = millis();
if(_active)
{
if(_update_alias)
{
Serial.println("reacquiring alias");
_update_alias = !_link->sendVerifyNID(NID, &DCC_NodeID);
if(!_update_alias) //message has gone through
_initial_blast = 10; //refresh status
return;
}
//see if we need to send out any periodic updates
if(_initial_blast)
{
Serial.print("Initial blast!! TRYING AGAIN! ");
Serial.println(_initial_blast);
_initial_blast--;
//this is a hack to make sure the first commands get through while DCS Vnode is being allocated
_dirty_speed = _dirty_FX = true; //send again!
}
if((new_time - _timer) >= 60000)
{
_timer = new_time;
_dirty_speed = true;
_dirty_FX = true;
}
if( _dirty_speed ) //if we are connected to CS, and either need to send an update or one minute has passed since last update:
{
sendSpeed();
}
if( _dirty_FX ) //if we are connected to CS, and either need to send an update or one minute has passed since last update:
{
sendFX();
}
}
OLCB_Datagram_Handler::update();
}
void navigate() {
while(1) {
if (current < destination) direction = 1;
else if (current > destination) direction = -1;
else if (current == destination) {
if (currentRoom > destinationRoom) direction = -1;
if (currentRoom < destinationRoom) direction = 1;
if (currentRoom == destinationRoom) {
sendSpeed(0.0, 0.0);
direction = 0;
Xprintf("Arrived at destintaion.\n");
return;
}
}
char string[50];
//sprintf(string, "Entered switch Curr: %d\n", current);
printf("Entered switch Curr: %d\n", current);
//Xprintf(string);
switch (current) {
case 0:
home();
break;
case 1:
MooreLobby();
break;
case 2:
MooreHallway();
break;
case 3:
MooreGRWLobby();
break;
case 4:
GRWHallway();
break;
case 5:
LevineHallway();
break;
case 6:
LevineLobby();
break;
default:
home();
break;
}
}
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
setupPlot();
socket = new TCPSocket(this);
connect(socket, SIGNAL(sendPosition(qreal,qreal,qreal,qreal,qreal)),
ui->widget, SLOT(paintPosition(qreal,qreal,qreal,qreal,qreal)));
connect(socket, SIGNAL(sendError(qreal,qreal,qreal,qreal)), this, SLOT(setError(qreal,qreal,qreal,qreal)));
connect(socket, SIGNAL(sendSpeed(qreal,qreal,qreal,qreal)), this, SLOT(setSpeed(qreal,qreal,qreal,qreal)));
connect(socket, SIGNAL(taskDone()), this, SLOT(enableButton()));
time = 0;
}
void MooreLobby() {
if (direction == 1) {
// Go forward for 3 feet
straightBlind(36.0);
// Correct Angle
correctAngle('B');
// Straight Blind for 3 feet, check SIG Center
straightBlind(36);
if (destination == 1 && destinationRoom == 0) {
currentRoom = 0;
return;
}
correctAngle('R');
// One-wall off the right wall at current distance until wall Lost
int lostL, lostR;
double wallL, wallR;
double currWall = 0;
urgStart(&urg);
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
lostR = 0;
while (!lostR) {
Xprintf("Waiting to lose Right wall.\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
// Go blind straight 7 feet
straightBlind(7*12.0);
// Correct angle between the white pillars
correctAngle('B');
// Go blind straight 5.5 feet
straightBlind(5.5*12.0);
// Turn left
turn90('L');
// Check Moore 100
if (destination == 1 && destinationRoom == 1) {
currentRoom = 1;
return;
}
// Go straight until right wall comes within range
wallR = 100000;
lostR = 1;
urgStart(&urg);
while(lostR || fabs(wallR) > (34* feet2mm)) {
Xprintf("Going straight until right wall closer than 3ft . . .\n");
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) {
sendSpeed(100.0, 100.0);
} else sendSpeed(0.0, 0.0);
}
urgStop(&urg);
// Straight to view right wall better
straightBlind(12);
// Align against right wall
correctAngle('R');
// Go straight until walls are less than 6 feet apart
wallL = wallR = 100000;
lostL = lostR = 1;
currWall = 0;
urgStart(&urg);
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0.0);
while (((fabs(wallL) + fabs(wallR)) > (6 * feet2mm)) || lostL) {
Xprintf("Searching for both walls to be less than 6ft apart . . .\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
currentRoom = -1;
current++; // Enter next feature
return;
}
if (direction == -1) {
// Correct angle against left wall
correctAngle('L');
// One wall along the left wall until it breaks
int lostL, lostR;
double wallL, wallR, currWall;
lostL = 0;
urgStart(&urg);
scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0);
while(!lostL) {
scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
// Turn Right
turn90('R');
// Go straight between the pillars
straightBlind(5*12);
// Correct angle
correctAngle('B');
// Straight blind into the opening in the left wall
straightBlind(10*12);
// Keep going straight blind until the left wall returns
wallL = 100000;
lostL = 1;
urgStart(&urg);
while(lostL) {
Xprintf("Going straight until left wall returns . . .\n");
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) {
sendSpeed(100.0, 100.0);
} else sendSpeed(0.0, 0.0);
}
urgStop(&urg);
// Correct angle
correctAngle('L');
// Keep going at current distance until Detkin lab door wall breaks
urgStart(&urg);
scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0);
lostL = 0;
while(!lostL) {
scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
// Straight
straightBlind(12);
current--;
currentRoom = -1;
return;
}
}
void handleFrSkyTelemetry(void)
{
if (!canSendFrSkyTelemetry()) {
return;
}
uint32_t now = millis();
if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
return;
}
lastCycleTime = now;
cycleNum++;
// Sent every 125ms
sendAccel();
sendVario();
sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms
if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
sendBaro();
}
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
sendThrottleOrBatterySizeAsRpm();
if (feature(FEATURE_VBAT)) {
sendVoltage();
sendVoltageAmp();
sendAmperage();
sendFuelLevel();
}
#ifdef GPS
if (sensors(SENSOR_GPS)) {
sendSpeed();
sendGpsAltitude();
sendSatalliteSignalQualityAsTemperature2();
}
#endif
// Send GPS information to display compass information
if (sensors(SENSOR_GPS) || (telemetryConfig->gpsNoFixLatitude != 0 && telemetryConfig->gpsNoFixLongitude != 0)) {
sendGPS();
}
sendTelemetryTail();
}
if (cycleNum == 40) { //Frame 3: Sent every 5s
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
/* Task to send the current setpoint to a remote node*/
void runLocalI2C(unsigned int *setSpeed) {
addr = 0x0;
sendSpeed(&addr, setSpeed);
// Delay10TCYx(20);
}
void MooreGRWLobby () {
if (direction == 1) {
currentRoom = 0;
// Return if set as home
if (destination == current && destinationRoom == 0) return;
// Move to 4 feet from left wall
moveToCenter(4 * feet2mm);
correctAngle('B');
int lostL = 1;
int lostR = 1;
double wallL = 10000;
double wallR = 10000;
// Keep going until both walls are found and less than 8ft apart
urgStart(&urg);
while (lostL || lostR || (fabs(wallL) + fabs(wallR)) > (8 * feet2mm)) {
Xprintf("Searching for both walls . . .\n");
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) {
sendSpeed(100.0, 100.0);
} else {
sendSpeed(0.0, 0.0);
}
}
urgStop(&urg);
current++;
currentRoom = -1;
return;
}
if (direction == -1) {
// Go straight until right wall appears at less than 5ft away
int lostL, lostR;
double wallL, wallR;
lostR = 1;
urgStart(&urg);
while (lostR || wallR > (5 * feet2mm)) {
Xprintf("Searching for right wall closer than 5ft . . .\n");
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) {
sendSpeed(100.0, 100.0);
} else {
sendSpeed(0.0, 0.0);
}
}
urgStop(&urg);
straightBlind(6);
correctAngle('R');
moveToCenter(-4.5 * feet2mm);
correctAngle('R');
lostL = lostR = 1;
urgStart(&urg);
// Go forward at 4.5ft from right wall until both walls appear less than 6ft apart
while (lostL || lostR || (fabs(wallL) + fabs(wallR)) > (6 * feet2mm)) {
scan_center(-4.5*feet2mm, &wallL, &wallR, &lostL, &lostR, 0);
}
urgStop(&urg);
sendSpeed(0.0, 0.0);
currentRoom = -1;
current--;
return;
}
}
void LevineLobby() {
if (direction == 1) {
// Go straight until wall are back
currentRoom = -1;
straightBlind(10*12);
turn90('L');
// Check elevators
currentRoom++;
if (current == destination && destinationRoom == currentRoom) return;
straightBlind(2.5*12);
correctAngle('R');
straightBlind(7*12);
turn90('R');
straightBlind(5*12);
correctAngle('R');
straightBlind(5*12);
correctAngle('R');
// Check for Levine Lobby
currentRoom++;
if (current == destination && destinationRoom == currentRoom) return;
/*
double currWall = 0;
int lostR = 0;
int lostL = 0;
double wallL, wallR;
// One-wall off the right wall at current distance until wall Lost
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
printf("Curr wall dist: %lf", currWall);
while (lostR == 0) {
printf("Waiting to lose Right wall.\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
straightBlind(6*12);
turn90('R');
straightBlind(7*12);
correctAngle('R');
// Set up position tracking
int lCount, rCount;
pollSensors(&lCount, &rCount);
position = 0.0;
lostL = lostR = 0;
// One-wall off the right wall at current distance for x feet
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
printf("Curr wall dist: %lf", currWall);
while (position < 12 * 12) {
printf("Waiting to go 12ft.\n");
lCount = rCount = 0;
pollSensors(&lCount, &rCount);
position = position + ((lCount + rCount) / 2.0) * ticks2inches;
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
correctAngle('R');
*/
currentRoom = -1;
current++;
return;
}
if (direction == -1) {
correctAngle('L');
double currWall = 0;
int lostR = 0;
int lostL = 0;
double wallL, wallR;
// One-wall off the left wall at current distance until wall Lost
urgStart(&urg);
scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0);
char string[50];
sprintf(string, "Curr wall dist: %lf", currWall);
Xprintf(string);
while (lostR == 0) {
Xprintf("Waiting to lose left wall.\n");
scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
straightBlind(5*12);
turn90('L');
correctAngle('R');
urgStart(&urg);
// One-wall off the right wall at current distance until wall Lost
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
char string2[50];
sprintf(string2, "Curr wall dist: %lf", currWall);
Xprintf(string2);
while (lostR == 0) {
Xprintf("Waiting to lose Right wall.\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
straightBlind(3.5*12);
turn90('R');
urgStart(&urg);
while (lostL || fabs(wallL) > (5*feet2mm)) {
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) sendSpeed(100.0, 100.0);
else sendSpeed(0.0, 0.0);
}
urgStop(&urg);
straightBlind(12);
correctAngle('L');
current--;
currentRoom = -1;
return;
}
}
void GRWHallway() {
if (direction == 1) {
// Set up distance counting
int lCount, rCount;
int lostL = 0;
int lostR = 0;
double wallL, wallR;
pollSensors(&lCount, &rCount);
position = 0.0;
lostL = lostR = 0;
currentRoom = -1;
int roomCount = 0;
// Move to center
moveToCenter(0.0);
pollSensors(&lCount, &rCount);
position = position + ((lCount + rCount) / 2.0) * ticks2inches;
correctAngle('B');
pollSensors(&lCount, &rCount);
urgStart(&urg);
// Keep going until both walls are lost or room found
while (!lostL || !lostR) {
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1) != -10) {
lCount = rCount = 0;
pollSensors(&lCount, &rCount);
position = position + ((lCount + rCount) / 2.0) * ticks2inches;
char string[70];
sprintf(string, "Going Straight. Pos: %lf\n", position);
Xprintf(string);
}
if (position > array[current].roomDist[roomCount]*12) {
currentRoom++;
roomCount++;
}
if (current == destination && currentRoom == destinationRoom) return;
}
urgStop(&urg);
straightBlind(1.5 * 12);
turn90('L');
sendSpeed(0.0, 0.0);
current++;
currentRoom = -1;
return;
}
if (direction == -1) {
straightBlind(6);
correctAngle('B');
moveToCenter(0.0);
correctAngle('B');
// Keep going down center of hall until wall both disappear
int lostL, lostR;
double wallL, wallR;
lostL = lostR = 0;
wallL = wallR = 1;
urgStart(&urg);
while(!(lostR || lostL || ((wallL > 5 * feet2mm) && (wallR > 5 * feet2mm)))) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until both walls further than 5ft or either is lost\n");
}
urgStop(&urg);
current--;
currentRoom = -1;
return;
}
}
int main(){
int i = 0;
int mapCount = 0, clearMapCount = 0, dumpCount=0;
int revFrameCount = 0;
#ifdef USE_NORTH
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED12LAT;
targetsGPS[maxTargets].lon = ADVANCED12LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
#else
targetsGPS[maxTargets].lat = ADVANCED4LAT;
targetsGPS[maxTargets].lon = ADVANCED4LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED1LAT;
targetsGPS[maxTargets].lon = ADVANCED1LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED2LAT;
targetsGPS[maxTargets].lon = ADVANCED2LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED3LAT;
targetsGPS[maxTargets].lon = ADVANCED3LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED8LAT;
targetsGPS[maxTargets].lon = ADVANCED8LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED7LAT;
targetsGPS[maxTargets].lon = ADVANCED7LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED6LAT;
targetsGPS[maxTargets].lon = ADVANCED6LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED11LAT;
targetsGPS[maxTargets].lon = ADVANCED11LON;
maxTargets++;
targetsGPS[maxTargets].lat = ADVANCED5LAT;
targetsGPS[maxTargets].lon = ADVANCED5LON;
maxTargets++;
#endif
maxTargetIndex=maxTargets-1;
for(i=0;i<maxTargets;i++){// this is converting all GPS point data to XY data.
targetListXY[i].x = GPSX(targetsGPS[i].lon, startLongitude);
targetListXY[i].y = GPSY(targetsGPS[i].lat, startLatitude);
}
currentXY.x = GPSX(gpsvar.longitude,startLongitude);// converts current robot X location compared to start longitude
currentXY.y = GPSY(gpsvar.latitude,startLatitude);// converts current robot Y location compared to start latitude
targetXY = targetListXY[currentTargetIndex];//sets first target GPS point
nextTargetIndex = (currentTargetIndex + 1)%maxTargets;//sets next target GPS point
nextXY = targetListXY[nextTargetIndex];// ??
previousXY.x = GPSX(startLongitude, startLongitude);// why?
previousXY.y = GPSY(startLatitude, startLatitude);//Why?
initRoboteq(); /* Initialize roboteq */
initGuide();//what is guide?
#ifdef USE_VISION // if USE_vision is defined, then initialize vision.
initVision();
#endif //USE_VISION
#ifdef USE_GPS// if USE_GPS is defined, then initialize GPS.
initGPS();
initParser();
#endif //USE_GPS
#ifdef USE_LIDAR// if USE_LIDAR is defined, then initialize LIDAR.
initObjects();
initSICK();
#endif //USE_LIDAR
#ifdef DEBUG_VISUALIZER// if defined, then use visualizer.
initVisualizer();
#endif //DEBUG_VISUALIZER
#ifdef USE_MAP//////>>>>>>>>>>>????
initMap(0,0,0);
#endif //USE_MAP
#ifdef DUMP_GPS// dump GPS data into file
FILE *fp;
fp = fopen("gpsdump.txt", "w");
#endif // DUMP_GPS
while(1){
double dir = 1.0;
double speed = 0.0, turn = 0.0;
static double turnBoost = 0.750;//Multiplier for turn. Adjust to smooth jerky motions. Usually < 1.0
static int lSpeed = 0, rSpeed = 0;//Wheel Speed Variables
if (joystick() != 0) {// is joystick is connected
if (joy0.buttons & LB_BTN) {// deadman switch, but what does joy0.buttons do?????????????????????????????????
speed = -joy0.axis[1]; //Up is negative on joystick negate so positive when going forward
turn = joy0.axis[0];
lSpeed = (int)((speed + turnBoost*turn)*maxSpeed);//send left motor speed
rSpeed = (int)((speed - turnBoost*turn)*maxSpeed);//send right motor speed
}else{ //stop the robot
rSpeed=lSpeed=0;
}
if(((joy0.buttons & B_BTN)||autoOn)&& (saveImage==0)){//what is the single & ???????????????????
saveImage =DEBOUNCE_FOR_SAVE_IMAGE;//save each image the camera takes, save image is an int declared in vision_nav.h
}else{
if (saveImage) saveImage--; // turn off if button wasn't pressed?
}
if(joy0.buttons & RB_BTN){//turn on autonmous mode if start??? button is pressed
autoOn = 1;
mode=1;
}
if(joy0.buttons & Y_BTN){ // turn off autonomous mode
autoOn = 0;
mode =0;
}
lastButtons = joy0.buttons;//is this just updating buttons?
} else{
// printf("No Joystick Found!\n");
rSpeed=lSpeed=0;
}
//
// printf("3: %f %f\n",BASIC3LAT,BASIC3LON);
// printf("4: %f %f\n",BASIC4LAT,BASIC4LON);
// printf("5: %f %f\n",BASIC5LAT,BASIC5LON);
// getchar();
#ifdef AUTO_SWAP//what is this
if((currentTargetIndex>1&&targetIndexMem!=currentTargetIndex)||!autoOn||!mode==3){
startTime=currentTime=(float)(clock()/CLOCKS_PER_SEC);
targetIndexMem = currentTargetIndex;
}else{
currentTime=(float)(clock()/CLOCKS_PER_SEC);
}
totalTime = currentTime-startTime;
if(totalTime>=SWAPTIME&&autoOn){
swap();
targetIndexMem = 0;
}
#endif //AUTO_SWAP
#ifdef USE_GPS
readGPS();
currentXY.x = GPSX(gpsvar.longitude,startLongitude);
currentXY.y = GPSY(gpsvar.latitude,startLatitude);
robotTheta = ADJUST_RADIANS(DEG2RAD(gpsvar.course));
#else
currentXY.x = 0.0;
currentXY.y = 0.0;
robotTheta = 0.0;
#endif //USE_GPS
if(autoOn&&!flagPointSet){//this whole thing?????
flagXY.x=currentXY.x+FLAG_X_ADJUST;
flagXY.y=currentXY.y;
flagPointSet=1;
startAutoTime=currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
}
if(autoOn){
currentAutoTime=(float)(clock()/CLOCKS_PER_SEC);
totalAutoTime = currentAutoTime-startAutoTime;
if(totalAutoTime>=MODE2DELAY){
mode1TimeUp=1;//what is mode1 time up?
}
printf("TIMEING\n");
}
// if(currentTargetIndex <= OPEN_FIELD_INDEX || currentTargetIndex >= maxTargetIndex){
if(currentTargetIndex <= OPEN_FIELD_INDEX){//if you are on your last target, then set approaching thresh, and dest thresh to larger values?
//OPEN_FIELD_INDEX is set to 0 above...?
approachingThresh=4.0;
destinationThresh=3.0;
}else{//otherwise set your thresholds to a bit closer.
// destinationThresh=1.0;
destinationThresh=0.75;
approachingThresh=2.5;
}
//mode1 = lane tracking and obstacle avoidance. mode 2 = vision, lane tracking, but guide to gps. its not primary focus.
//mode3= gps mode in open field, but vision is toned down to not get distracted by random grass.
//mode 4= flag tracking
if(guide(currentXY, targetXY, previousXY, nextXY, robotTheta, robotWidth, 1)&& !allTargetsReached){//If target reached and and not all targets reached
printf("REACHED TARGET\n");
initGuide();// reset PID control stuff. problably resets all control variables.
previousXY = targetXY;//update last target
if(currentTargetIndex == maxTargetIndex){ //seeing if you are done with all targets.
allTargetsReached = 1;
}else{//otherwise update all the target information
currentTargetIndex = (currentTargetIndex + 1);
nextTargetIndex = (currentTargetIndex + 1)% maxTargets;
targetXY = targetListXY[currentTargetIndex];
nextXY = targetListXY[nextTargetIndex];
}
}
if((autoOn&&(currentTargetIndex == 0&&!approachingTarget&&!mode1TimeUp))||allTargetsReached){
//if autonomous, and on first target, and not not approaching target, and not mode 1 time up, or reached last target.
mode =1;//wtf is mode
distanceMultiplier = 50;//wthis is how heavily to rely on vision
} else if((autoOn&¤tTargetIndex == 0&&mode1TimeUp)||(autoOn&&approachingTarget&&(currentTargetIndex<=OPEN_FIELD_INDEX||currentTargetIndex>=maxTargetIndex-END_LANE_INDEX))){
mode =2;
distanceMultiplier = 50;
} else if((autoOn&¤tTargetIndex!=0)){
mode =3;
distanceMultiplier = 12;
}
flagPointDistance = D((currentXY.x-flagXY.x),(currentXY.y-flagXY.y));// basically the distance formula, but to what? what flags GPS point?
if(allTargetsReached&&flagPointDistance<FLAG_DIST_THRESH){
mode =4;// what is mode
}
#ifdef FLAG_TESTING
/*FLAG TESTING*/
mode=4;
#endif //FLAG_TESTING
/*Current Target Heading PID Control Adjustment*/
cvar.lookAhead = 0.00;//?
cvar.kP = 0.20; cvar.kI = 0.000; cvar.kD = 0.15;
turn = cvar.turn;
int bestVisGpsMask = 99;
int h = 0;
double minVisGpsTurn = 9999;
for(h=0;h<11;h++){
if(fabs((cvar.turn-turn_angle[h]))<minVisGpsTurn){
minVisGpsTurn=fabs((cvar.turn-turn_angle[h]));
bestVisGpsMask = h;
}
}
bestGpsMask = bestVisGpsMask;
// printf("bvg: %d \n", bestVisGpsMask);
// printf("vgt: %f cv3: %f\n", minVisGpsTurn,cvar3.turn);
#ifdef USE_VISION
// double visTurnBoost = 0.50;
double visTurnBoost = 1.0;
if(imageProc(mode) == -1) break;
if(mode==1||mode==2){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}else if(mode==3 && fabs(turn_angle[bestmask])>0.70){
turn = turn_angle[bestmask];
turn *= visTurnBoost;
}
#endif //USE_VISION
#ifdef USE_LIDAR
updateSick();
// findObjects();
#endif //USE_LIDAR
#ifdef USE_COMBINED_BUFFER//??????????
#define WORSTTHRESH 10
#define BESTTHRESH 3
if(mode==4){
#ifdef USE_NORTH
turn = (0.5*turn_angle[bestBlueMask]+0.5*turn_angle[bestRedMask]);
#else
turn = (0.65*turn_angle[bestBlueMask]+0.35*turn_angle[bestRedMask]);
#endif
turn *= 0.75;
}
combinedTargDist = cvar.targdist;
if(((approachingTarget||inLastTarget)&¤tTargetIndex>OPEN_FIELD_INDEX
&¤tTargetIndex<maxTargetIndex-END_LANE_INDEX)||(MAG(howbad[worstmask]-howbad[bestmask]))<BESTTHRESH||mode==4){
getCombinedBufferAngles(0,0);//Don't Use Vision Radar Data
}else{
getCombinedBufferAngles(0,1);//Use Vision Radar Data
}
if(combinedBufferAngles.left != 0 || combinedBufferAngles.right !=0){
if(mode == 1 || mode==2 || mode==3 || mode==4){
// if(mode == 1 || mode==2 || mode==3){
// if(mode==2 || mode==3){
// if(mode==3){
if(fabs(combinedBufferAngles.right)==fabs(combinedBufferAngles.left)){
double revTurn;
double revDistLeft, revDistRight;
int revIdx;
if(fabs(turn)<0.10) dir = -1.0;
if(fabs(combinedBufferAngles.left)>1.25) dir = -1.0;
if(dir<0){
revIdx = 540-RAD2DEG(combinedBufferAngles.left)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistLeft = LMSdata[revIdx];
revIdx = 540-RAD2DEG(combinedBufferAngles.right)*4;
revIdx = MIN(revIdx,1080);
revIdx = MAX(revIdx,0);
revDistRight = LMSdata[revIdx];
if(revDistLeft>=revDistRight){
revTurn = combinedBufferAngles.left;
}else {
revTurn = combinedBufferAngles.right;
}
turn = revTurn;
}else{
turn = turn_angle[bestmask];
}
} else if(fabs(combinedBufferAngles.right-turn)<fabs(combinedBufferAngles.left-turn)){
// } else if(turn<=0){
turn = combinedBufferAngles.right;
}else {
turn = combinedBufferAngles.left;
}
}
}
#endif //USE_COMBINED_BUFFER
if(dir<0||revFrameCount!=0){
dir = -1.0;
revFrameCount = (revFrameCount+1)%REVFRAMES;
}
// turn *= dir;
turn = SIGN(turn) * MIN(fabs(turn), 1.0);
speed = 1.0/(1.0+1.0*fabs(turn))*dir;
speed = SIGN(speed) * MIN(fabs(speed), 1.0);
if(!autoOn){
maxSpeed = 60;
targetIndexMem = 0;
}else if(dir<0){
maxSpeed = 30;
}else if(mode<=2||(mode==3 && fabs(turn_angle[bestmask])>0.25)){
maxSpeed = 60 - 25*fabs(turn);
// maxSpeed = 70 - 35*fabs(turn);
// maxSpeed = 90 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
}else if(mode==4){
maxSpeed = 45-20*fabs(turn);
}else{
maxSpeed = 85 - 50*fabs(turn);
// maxSpeed = 100 - 65*fabs(turn);
// maxSpeed = 110 - 70*fabs(turn);
// maxSpeed = 120 - 85*fabs(turn);
}
if(autoOn){
lSpeed = (speed + turnBoost*turn) * maxSpeed;
rSpeed = (speed - turnBoost*turn) * maxSpeed;
}
#ifdef DEBUG_MAIN
printf("s:%.4f t: %.4f m: %d vt:%f dir:%f tmr: %f\n", speed, turn, mode, turn_angle[bestmask], flagPointDistance, totalAutoTime);
#endif //DEBUG_MAIN
#ifdef DUMP_GPS
if(dumpCount==0){
if (fp != NULL) {
fprintf(fp, "%f %f %f %f %f\n",gpsvar.latitude,gpsvar.longitude, gpsvar.course, gpsvar.speed, gpsvar.time);
}
}
dumpCount = dumpCount+1%DUMPGPSDELAY;
#endif //DUMP_GPS
#ifdef DEBUG_TARGET
debugTarget();
#endif //DEBUG_TARGET
#ifdef DEBUG_GUIDE
debugGuide();
#endif //DEBUG_GUIDE
#ifdef DEBUG_GPS
debugGPS();
#endif //DEBUG_GPS
#ifdef DEBUG_LIDAR
debugSICK();
#endif //DEBUG_LIDAR
#ifdef DEBUG_BUFFER
debugCombinedBufferAngles();
#endif //DEBUG_BUFFE
#ifdef DEBUG_VISUALIZER
robotX = currentXY.x;
robotY = currentXY.y;
robotTheta = robotTheta;//redundant I know....
targetX = targetXY.x;
targetY = targetXY.y;
// should probably pass the above to the function...
paintPathPlanner(robotX,robotY,robotTheta);
showPlot();
#endif //VISUALIZER
#ifdef USE_MAP
if(mapCount==0){
// mapRobot(currentXY.x,currentXY.y,robotTheta);
if(clearMapCount==0) clearMapSection(currentXY.x,currentXY.y,robotTheta);
else clearMapCount = (clearMapCount+1)%CLEARMAPDELAY;
mapVSICK(currentXY.x,currentXY.y,robotTheta);
// mapVSICK(0,0,0);
#ifdef USE_LIDAR
mapSICK(currentXY.x,currentXY.y,robotTheta);
#endif
showMap();
// printf("MAPPING\n");
}
mapCount= (mapCount+1)%MAPDELAY;
#endif //USE_MAP
sendSpeed(lSpeed,rSpeed);
Sleep(5);
}
#ifdef DUMP_GPS
fclose(fp);
#endif
return 0;
}
void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
if (!frskyTelemetryEnabled) {
return;
}
uint32_t now = millis();
if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
return;
}
lastCycleTime = now;
cycleNum++;
// Sent every 125ms
sendAccel();
sendVario();
sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms
if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
sendBaro();
}
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
sendThrottleOrBatterySizeAsRpm(rxConfig, deadband3d_throttle);
if (feature(FEATURE_VBAT)) {
sendVoltage();
sendVoltageAmp();
sendAmperage();
sendFuelLevel();
}
#ifdef GPS
if (sensors(SENSOR_GPS)) {
sendSpeed();
sendGpsAltitude();
sendSatalliteSignalQualityAsTemperature2();
sendGPSLatLong();
}
else {
sendFakeLatLongThatAllowsHeadingDisplay();
}
#else
sendFakeLatLongThatAllowsHeadingDisplay();
#endif
sendTelemetryTail();
}
if (cycleNum == 40) { //Frame 3: Sent every 5s
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
void LevineHallway() {
if (direction == 1) {
currentRoom = -1;
correctAngle('R');
straightBlind(3*12);
currentRoom++;
// Stop for Recycling Center
if (current == destination && currentRoom == destinationRoom) return;
moveToCenter(0.0);
correctAngle('L');
straightBlind(8*12);
moveToCenter(0.0);
correctAngle('B');
// Keep going until left wall too far
int lostL, lostR;
lostL = 0;
double wallL, wallR;
urgStart(&urg);
while (lostL != 1) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until left lost\n");
}
urgStop(&urg);
current++;
currentRoom = -1;
return;
}
if (direction == -1) {
moveToCenter(0.0);
correctAngle('B');
// Keep going until hall width < 7ft
int lostL, lostR;
lostL = 0;
double wallL = 10000;
double wallR = 10000;
urgStart(&urg);
while (fabs(wallL) + fabs(wallR) > (7 * feet2mm)) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until hallway narrows to 7ft\n");
}
urgStop(&urg);
moveToCenter(0.0);
correctAngle('B');
// Go until right wall breaks
urgStart(&urg);
while (lostR != 1) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until right wall breaks\n");
}
urgStop(&urg);
straightBlind(20);
turn90('R');
// Go straight until walls are back
lostL = lostR = 1;
urgStart(&urg);
while (!(!lostL && !lostR)) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until right lost\n");
}
urgStop(&urg);
sendSpeed(0.0, 0.0);
current--;
currentRoom = -1;
return;
}
}
