void loop() {
while(1){
sendString(Time, Latitude, Longitude, Altitude, internalTemp, externalTemp, NumSatalites, pressure, geigerCount, ADCValue[0]);
getADCValue(0);
getTemperature();
readBMP085();
readGeigerCount();
//printf("UV: %i Temp: %F Lat: %F Long: %F Alt: %F SatNu: %i Pressure %i iTemp %F GMC: %i\n", ADCValue[0], externalTemp, Latitude, Longitude, Altitude, NumSatalites, pressure, internalTemp, geigerCount);
closeRadio();
getGPS();
//sleep(1);
}
}
MyRobot::MyRobot() : DifferentialWheels()
{
_time_step = 64;
_left_speed = 0;
_right_speed = 0;
// Get and enable the compass device
_my_compass = getCompass("compass");
_my_compass->enable(_time_step);
// Get and enable the gps device
_my_gps = getGPS("gps");
_my_gps->enable(_time_step);
//Get and enable the distance sensors
_distance_sensor[0] = getDistanceSensor("ds0");
_distance_sensor[1] = getDistanceSensor("ds1");
_distance_sensor[2] = getDistanceSensor("ds2");
_distance_sensor[3] = getDistanceSensor("ds3");
_distance_sensor[4] = getDistanceSensor("ds4");
_distance_sensor[5] = getDistanceSensor("ds5");
_distance_sensor[6] = getDistanceSensor("ds6");
_distance_sensor[7] = getDistanceSensor("ds7");
_distance_sensor[8] = getDistanceSensor("ds8");
_distance_sensor[9] = getDistanceSensor("ds9");
_distance_sensor[10] = getDistanceSensor("ds10");
_distance_sensor[11] = getDistanceSensor("ds11");
_distance_sensor[12] = getDistanceSensor("ds12");
_distance_sensor[13] = getDistanceSensor("ds13");
_distance_sensor[14] = getDistanceSensor("ds14");
_distance_sensor[15] = getDistanceSensor("ds15");
for(int i=0; i<NUM_DISTANCE_SENSOR; i++)
{
_distance_sensor[i]->enable(_time_step);
}
//Start the robot with the mode FORWARD
_mode = FORWARD;
}
// 0.000011479429428388439 gps points per meter
void waypointManager(void){
waypoint * newWP = (waypoint *) malloc(sizeof(waypoint));
dataGPS curPosGPS;
//int gpsRet;
float turnHeading;
double tolerance = 3; // tolerance in meters
printf("Beginning\r\n");
addWaypointxy(-87.322314, 39.483694);
newWP = getCurrentWaypoint();
printf("\r\nTrying to initialize GPS");
//gpsRet = init_GPS();
curPosGPS = getGPS(gpsRet);
if(!curPosGPS.valid){
for(int i = 0; i < 10 && !curPosGPS.valid; i++){
sleep(5);
printf("\r\nWaiting for valid GPS data....\r\n");
curPosGPS = getGPS(gpsRet);
}
}
waypoint * curPosWP = (waypoint *) malloc(sizeof(waypoint));
curPosWP->x = curPosGPS.x;
curPosWP->y = curPosGPS.y;
turnHeading = angleBetweenPoints(*newWP, *curPosWP);
turn(turnHeading);
// at this point we should be pointing in the right direction
double line_mb[2];
findLine(*curPosWP, * newWP, line_mb);
printf("\r\nCurrent position: x %lf, y %lf\r\n", curPosWP->x, curPosWP->y);
printf("Waypoint to move to: x %lf, y %lf\r\n", newWP->x, newWP->y);
printf("Turn heading: %f\r\n", turnHeading);
printf("Line: m %lf, b %lf\r\n", line_mb[0], line_mb[1]);
int waypointsLeft = 1;
while(keepGoing && waypointsLeft){
while(keepGoing && distanceBetweenPoints(*curPosWP, *newWP) > tolerance*0.000011479429428388439){
// go straight
printf("Going straight\r\ndistance to target: %lf\r\ntolerance: %lf\r\n", distanceBetweenPoints(*curPosWP, *newWP), tolerance*0.000011479429428388439);
gpio_set_value(LEFT_FWD_GPIO, 1);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 1);
gpio_set_value(RIGHT_BCK_GPIO, 0);
curPosGPS = getGPS(gpsRet);
curPosWP->x = curPosGPS.x;
curPosWP->y = curPosGPS.y;
if(!bbCheck(line_mb[0], line_mb[1], * curPosWP, tolerance)){
returnToPreviousWaypoint();
printf("We're outside of the bounds. Returning to the previous waypoint\r\n");
break; // out of the bounding box
}
sleep(2);
}
printf("We've hit the waypoint! Stopping the motors...\r\n");
gpio_set_value(LEFT_FWD_GPIO, 0);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 0);
gpio_set_value(RIGHT_BCK_GPIO, 0);
waypointsLeft = (advanceToNextWaypoint() != -1); // don't keep going if we've reached the end of the waypoints
newWP = getCurrentWaypoint(); // if we've already advanced past the end this will give us the last WP, but
// we will break out of this loop anyway
printf("KeepGoing: %d...should be 0 if we've hit the end of the waypoints", keepGoing);
}
free(curPosWP);
free(newWP);
}
dataGPS getGPSlocation(void){
return getGPS(gpsRet);
}
boolean Adafruit_FONA::getGPS(float *lat, float *lon, float *speed_kph, float *heading, float *altitude) {
char gpsbuffer[120];
// we need at least a 2D fix
if (GPSstatus() < 2)
return false;
// grab the mode 2^5 gps csv from the sim808
uint8_t res_len = getGPS(32, gpsbuffer, 120);
// make sure we have a response
if (res_len == 0)
return false;
if (_type == FONA3G_A || _type == FONA3G_E) {
// Parse 3G respose
// +CGPSINFO:4043.000000,N,07400.000000,W,151015,203802.1,-12.0,0.0,0
// skip beginning
char *tok;
// grab the latitude
char *latp = strtok(gpsbuffer, ",");
if (! latp) return false;
// grab latitude direction
char *latdir = strtok(NULL, ",");
if (! latdir) return false;
// grab longitude
char *longp = strtok(NULL, ",");
if (! longp) return false;
// grab longitude direction
char *longdir = strtok(NULL, ",");
if (! longdir) return false;
// skip date & time
tok = strtok(NULL, ",");
tok = strtok(NULL, ",");
// only grab altitude if needed
if (altitude != NULL) {
// grab altitude
char *altp = strtok(NULL, ",");
if (! altp) return false;
*altitude = atof(altp);
}
// only grab speed if needed
if (speed_kph != NULL) {
// grab the speed in km/h
char *speedp = strtok(NULL, ",");
if (! speedp) return false;
*speed_kph = atof(speedp);
}
// only grab heading if needed
if (heading != NULL) {
// grab the speed in knots
char *coursep = strtok(NULL, ",");
if (! coursep) return false;
*heading = atof(coursep);
}
double latitude = atof(latp);
double longitude = atof(longp);
// convert latitude from minutes to decimal
float degrees = floor(latitude / 100);
double minutes = latitude - (100 * degrees);
minutes /= 60;
degrees += minutes;
// turn direction into + or -
if (latdir[0] == 'S') degrees *= -1;
*lat = degrees;
// convert longitude from minutes to decimal
degrees = floor(longitude / 100);
minutes = longitude - (100 * degrees);
minutes /= 60;
degrees += minutes;
// turn direction into + or -
if (longdir[0] == 'W') degrees *= -1;
*lon = degrees;
} else if (_type == FONA808_V2) {
// Parse 808 V2 response. See table 2-3 from here for format:
// http://www.adafruit.com/datasheets/SIM800%20Series_GNSS_Application%20Note%20V1.00.pdf
// skip GPS run status
char *tok = strtok(gpsbuffer, ",");
if (! tok) return false;
// skip fix status
tok = strtok(NULL, ",");
if (! tok) return false;
// skip date
tok = strtok(NULL, ",");
if (! tok) return false;
// grab the latitude
char *latp = strtok(NULL, ",");
if (! latp) return false;
// grab longitude
char *longp = strtok(NULL, ",");
if (! longp) return false;
*lat = atof(latp);
*lon = atof(longp);
// only grab altitude if needed
if (altitude != NULL) {
// grab altitude
char *altp = strtok(NULL, ",");
if (! altp) return false;
*altitude = atof(altp);
}
// only grab speed if needed
if (speed_kph != NULL) {
// grab the speed in km/h
char *speedp = strtok(NULL, ",");
if (! speedp) return false;
*speed_kph = atof(speedp);
}
// only grab heading if needed
if (heading != NULL) {
// grab the speed in knots
char *coursep = strtok(NULL, ",");
if (! coursep) return false;
*heading = atof(coursep);
}
}
else {
// Parse 808 V1 response.
// skip mode
char *tok = strtok(gpsbuffer, ",");
if (! tok) return false;
// skip date
tok = strtok(NULL, ",");
if (! tok) return false;
// skip fix
tok = strtok(NULL, ",");
if (! tok) return false;
// grab the latitude
char *latp = strtok(NULL, ",");
if (! latp) return false;
// grab latitude direction
char *latdir = strtok(NULL, ",");
if (! latdir) return false;
// grab longitude
char *longp = strtok(NULL, ",");
if (! longp) return false;
// grab longitude direction
char *longdir = strtok(NULL, ",");
if (! longdir) return false;
double latitude = atof(latp);
double longitude = atof(longp);
// convert latitude from minutes to decimal
float degrees = floor(latitude / 100);
double minutes = latitude - (100 * degrees);
minutes /= 60;
degrees += minutes;
// turn direction into + or -
if (latdir[0] == 'S') degrees *= -1;
*lat = degrees;
// convert longitude from minutes to decimal
degrees = floor(longitude / 100);
minutes = longitude - (100 * degrees);
minutes /= 60;
degrees += minutes;
// turn direction into + or -
if (longdir[0] == 'W') degrees *= -1;
*lon = degrees;
// only grab speed if needed
if (speed_kph != NULL) {
// grab the speed in knots
char *speedp = strtok(NULL, ",");
if (! speedp) return false;
// convert to kph
*speed_kph = atof(speedp) * 1.852;
}
// only grab heading if needed
if (heading != NULL) {
// grab the speed in knots
char *coursep = strtok(NULL, ",");
if (! coursep) return false;
*heading = atof(coursep);
}
// no need to continue
if (altitude == NULL)
return true;
// we need at least a 3D fix for altitude
if (GPSstatus() < 3)
return false;
// grab the mode 0 gps csv from the sim808
res_len = getGPS(0, gpsbuffer, 120);
// make sure we have a response
if (res_len == 0)
return false;
// skip mode
tok = strtok(gpsbuffer, ",");
if (! tok) return false;
// skip lat
tok = strtok(NULL, ",");
if (! tok) return false;
// skip long
tok = strtok(NULL, ",");
if (! tok) return false;
// grab altitude
char *altp = strtok(NULL, ",");
if (! altp) return false;
*altitude = atof(altp);
}
return true;
}
//this function is called every 50ms. it sends the appropriate command to the quadcoper
LOCAL void ICACHE_FLASH_ATTR
user_udp_send(void)
{
char DeviceBuffer[40] = {0};
char hwaddr[6];
struct ip_info ipconfig;
unsigned long pitch;
unsigned long speed;
const char udp_remote_ip[4] = { 255, 255, 255, 255};
os_memcpy(user_udp_espconn.proto.udp->remote_ip, udp_remote_ip, 4); // ESP8266 udp remote IP need to be set everytime we call espconn_sent
user_udp_espconn.proto.udp->remote_port = 5556; // ESP8266 udp remote port need to be set everytime we call espconn_sent
wifi_get_macaddr(STATION_IF, hwaddr);
//detect adc button push, and increase state
if(system_adc_read() < 100 & adc_read_last > 100){
flystate = (flystate + 1)%FLYSTATECOUNT;
}
//initialise navigational data if necessary
if(flystate == FLYSTATE_INITNAV){
os_sprintf(DeviceBuffer, "AT*CONFIG=\"general:navdata_demo\",\"TRUE\"\\r");
//os_sprintf(DeviceBuffer, "AT*CONFIG=%d%s%s\r", "\"general:navdata_demo\"","\"TRUE\"");
flystate++;
//acknowledge something
}else if(flystate == FLYSTATE_ACK1){
os_sprintf(DeviceBuffer, "AT*CTRL=0\r");
flystate++;
// tell drone its flying with outdoor shell
}else if(flystate == FLYSTATE_OUTDOOR){
os_sprintf(DeviceBuffer, "AT*CONFIG=%d,%s,%s\r", sequence++, "\"control:flight_without_shell\"","\"TRUE\"");
flystate++;
//AT*CONFIG=605,"control:flight_without_shell","TRUE"
}else if(flystate == FLYSTATE_OUTDOORSHELL){
os_sprintf(DeviceBuffer, "AT*CONFIG=%d,%s,%s\r", sequence++, "\"control:outdoor\"","\"TRUE\"");
flystate++;
// takeoff!
}else if(flystate == FLYSTATE_TAKEOFF){
os_sprintf(DeviceBuffer, "%s%d%s\r", ATREF, sequence++, TAKEOFF);
needsCalibration = 1;
//operate manual control
}else if(flystate == FLYSTATE_MANUAL1 | flystate == FLYSTATE_MANUAL2){
forward = !GPIO_INPUT_GET(5);
left = !GPIO_INPUT_GET(0);
right = !GPIO_INPUT_GET(4);
if(needsCalibration){
os_sprintf(DeviceBuffer, "AT*CALIB=%d,%d\r", sequence++, 0);
needsCalibration = 0;
}else{
speed = forward*neg50asSingle;
//determine values frim input
if(left & !right)
pitch = neg50asSingle;
else if(right & !left)
pitch = d50asSingle;
else
pitch = 0;
os_sprintf(DeviceBuffer, "%s%d,%d,%d,%d,%d,%d\r", ATPCMD, sequence++, (left || right || forward),
0, speed,0, pitch);
//os_sprintf(DeviceBuffer, "%s" MACSTR "!" , ESP8266_MSG, MAC2STR(hwaddr));
}
//otherwse perform pathing with reference to GPS data
}else if(flystate == FLYSTATE_AUTOMATIC)
os_sprintf(DeviceBuffer, "AT*COMWDG=%d\r", sequence++);
//send ftrim if grounded
else if(flystate == FLYSTATE_GROUNDED_START)
os_sprintf(DeviceBuffer, "AT*FTRIM=%d\r", sequence++);
//send land command
else if(flystate == FLYSTATE_GROUNDED_END)
os_sprintf(DeviceBuffer, "%s%d%s\r", ATREF, sequence++, LAND);
os_printf("LAT0: %d LONG0: %d, TIME0: %d LAT1: %d LONG1: %d TIME1: %d\n", (int)(getGPS(0, RMC_LAT)*1000), (int)(getGPS(0, RMC_LONG)*1000), (int)getGPS(0, RMC_TIME), (int)(getGPS(1, RMC_LAT)*1000), (int)(getGPS(1, RMC_LONG)*1000), (int)getGPS(1, RMC_TIME));
//os_printf("TEST: %d\n", (int)10.0);
//update adc_read_last
adc_read_last = system_adc_read();
//send the buffer
espconn_sent(&user_udp_espconn, DeviceBuffer, os_strlen(DeviceBuffer));
}
