void parseHttp() {
String line = Serial.readStringUntil('\n');
if (line.startsWith("Content-Length: ")) {
sscanf(line.c_str() + 16, "%d", &contentLength);
} else if (line.startsWith("GET")) {
method = GET;
path = parsePath(line);
} else if (line.startsWith("PUT")) {
method = PUT;
path = parsePath(line);
} else if (line == "\r") {
if (path.startsWith("/reset")) {
sendResponse(200);
setup();
} else if (path.startsWith("/sensors") && method == GET) {
getSensors();
} else if (path.startsWith("/leds") && (method == GET)) {
getLeds();
} else if (path.startsWith("/leds") && (method == PUT)) {
putLeds();
} else if (path.startsWith("/settings") && (method == GET)) {
getSettings();
} else if (path.startsWith("/settings") && (method == PUT)) {
putSettings();
} else {
sendResponse(404);
}
contentLength = 0;
}
}
void Plugin::upload() {
if (g_middleware == "")
return;
char uuid_c[UUID_LENGTH+1];
char val_c[16];
for (int8_t i=0; i<getSensors(); i++) {
// uuid configured?
getUuid(uuid_c, i);
if (strlen(uuid_c) > 0) {
float val = getValue(i);
if (isnan(val))
break;
dtostrf(val, -4, 2, val_c);
String uri = g_middleware + F("/data/") + String(uuid_c) + F(".json?value=") + String(val_c);
http.begin(uri);
int httpCode = http.POST("");
if (httpCode > 0) {
http.getString();
}
DEBUG_MSG(getName().c_str(), "POST %d %s\n", httpCode, uri.c_str());
http.end();
}
}
}
void Plugin::getPluginJson(JsonObject* json) {
JsonArray& sensorlist = json->createNestedArray("sensors");
for (int8_t i=0; i<getSensors(); i++) {
JsonObject& data = sensorlist.createNestedObject();
getSensorJson(&data, i);
}
}
virtual std::unique_ptr<DroneControllerBase> createController() override
{
unique_ptr<DroneControllerBase> controller(new MavLinkDroneController());
auto mav_controller = static_cast<MavLinkDroneController*>(controller.get());
mav_controller->initialize(connection_info_, & getSensors(), true);
return controller;
}
void AHRSClass::update(float dt)
{
getSensors();
MerayoCalibClass::apply(&scaledSensorData.x, &scaledSensorData.y, &scaledSensorData.z, &accCalibData);
fuse(dt);
transformOrientation();
}
int process_cmd(int index){
printf ("curr_cmd:%d\n", curr_cmd);
printf ("prev_cmd:%d\n", prev_cmd);
//If the command sent to car last time is not the same as the current command then send it
if ((curr_cmd != prev_cmd)||(curr_cmd!= FORWARD)){
if (curr_cmd == FORWARD){
printf ("*SEND FORWARD COMMAND\n");
send_cmd (10, 0);
}
else if (curr_cmd == TURN_LEFT){
printf ("*SEND TURN LEFT COMMAND\n");
left_flag = right_flag = front_flag = 0; //Reset flags to 0
send_cmd(0,0);
sleep(1);
send_cmd (0, 270);
sleep(3);
}
else if (curr_cmd == TURN_RIGHT){
printf ("*SEND TURN RIGHT COMMAND\n");
if ((index==4) || (index==5)){
//printf ("front: %d, temp_sc_front: %d\n", front, temp_sc_front_dist);
int i = 0;
while (front >= temp_sc_front_dist){
getSensors();
}
printf ("front: %d, temp_sc_front: %d\n", front, temp_sc_front_dist);
//printf ("new wall before turning right - is_left: %d, is_right: %d, is_front: %d", left,right,front);
//printf ("avg_front: %d, temp_sc_front/2: %d\n", avg_front, temp_sc_front_dist/2);
}
left_flag = right_flag = front_flag = 0; //Reset flags to 0
send_cmd(0,0);
sleep(1);
send_cmd (0, 90);
sleep(3);
}
else if (curr_cmd == SLOW){
printf ("SEND SLOW DOWN COMMAND\n");
send_cmd (01, 0);
}
else
return 0;
return 1; //succes: command sent
}
else
return 0;
}
void main_planner (){
//TODO: This function must be called in an infinite while loop. Remove the while(true) when integrated with rest of software
getSensors();
///If all 3 sonar sensors are ready to be read from memory (flag byte for each sensor is 1)
//If currently turning do not do anything
//BEWARE between reading all flags as 1 and setting turning_flag to 1 and sending turning command, memory may update again! therefore disabling the turning_flag
if (left_flag == 1 && right_flag == 1 && front_flag == 1){
left_flag = right_flag = front_flag = 0; //Reset flags to 0 so we start updating our globals again
update_current_dist_to_wall();
int index = check_environment();
if ((state[index] == (MAX_COUNT/2)) && index >= 4 && index <= 7)
{
printf ("Entered state change segment, state: %d\n", index);
temp_sc_front_dist = front/2;
if(front<500)
front_thresh = front;
printf ("temp_front_dist: %d\n", temp_sc_front_dist);
prev_cmd = curr_cmd;
curr_cmd = SLOW;
process_cmd(0);
}
if ((state[index] == (MAX_COUNT/2)) && index == 2)
{
printf ("Entered pre state 2\n");
//temp_sc_front_dist = front;
//if(front<500)
//front_thresh = max(front-50,100);
((front-50)<100)? front_thresh = 100 : ((front_thresh = front - 50),state[index] = 0);
printf ("front thresh: %d\n", front_thresh);
prev_cmd = curr_cmd;
curr_cmd = SLOW;
process_cmd(0);
}
//check if incremented stated has reached Possible_State_Change_Count
//change front threshold to 100
//go to new function
//new function:
//if that new state requires turning then get distance to front and devide by two and store it in global
//
if (state[index] >= MAX_COUNT){
printf ("STATE:%d\n", index);
track_sensors(index);
Detect_Wall_Turn_Algo(index);
process_cmd(index);
check_drift(index);
}
return_from_drift(index);
}
}//main_planner
// Lidar APIs
virtual LidarData getLidarData(const std::string& lidar_name) const
{
const LidarBase* lidar = nullptr;
// Find lidar with the given name (for empty input name, return the first one found)
// Not efficient but should suffice given small number of lidars
uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
for (uint i = 0; i < count_lidars; i++)
{
const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
{
lidar = current_lidar;
break;
}
}
if (lidar == nullptr)
throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
return lidar->getOutput();
}
/*virtual*/ void h_Robot::move()
{
// Probe its nearby environment
QVector<qreal> sensorsIntensities = getSensors();
// Run the controller
neuralNet->run(sensorsIntensities, maxSensorIntensity);
// Update position
updatePosition();
// Prevent the robot from leaving the world
checkBorder(s->rob.borderMode);
}
int main(void){
char data[9];
initRobot();
if(getSensors(&data[0]) == -1) printf("chyba\n");
// if(getULT(&data[0]) == -1) printf("chyba\n");
// int data[10];
// int dataULT[10];
// readSensors(&data[0]);
// readULT(&dataULT[0]);
printf("%d\n",data[1]);
// LED3(0);
return 1;
}
bool Plugin::loadConfig() {
File configFile = SPIFFS.open("/" + getName() + ".config", "r");
if (configFile.size() == _size) {
DEBUG_MSG(getName().c_str(), "loading config\n", _size);
configFile.read((uint8_t*)_devices, _size);
}
else if (configFile.size() == 0)
DEBUG_MSG(getName().c_str(), "config not found\n", _size);
else
DEBUG_MSG(getName().c_str(), "config size mismatch\n", _size);
for (int8_t sensor = 0; sensor<getSensors(); sensor++)
if (strlen(_devices[sensor].uuid) != UUID_LENGTH)
_devices[sensor].uuid[0] = '\0';
configFile.close();
return true;
}
/**************************************************************************************************
######################################## Planner Algorithms ########################################
*/
int check_environment(){
int i = -1;
//If reached this point, then car just finished turning, now move forward
if (turning_flag == 1){
count_right = 0;
turning_flag = 0;
while (!front_flag)
{
getSensors();
}
if (front < 300)
{
printf ("\t\t00-Finished Turning, Wall ahead.. \n");
sleep(1);
turning_flag = -1;
}
else
{
//Move Forward
printf ("\n*************************************\n*************************************\n");
printf ("\t\t00-Finished Turning, stop then move forward\n");
sleep(1);
prev_cmd = curr_cmd;
curr_cmd = FORWARD;
process_cmd(0);
}
}
//After turning, if wall is seen on the right then proceed with normal algorithm and disable turning flag
//Otherwise move forward and DO NOT DO ANYTHING ELSE until a wall on the right is detected
if (turning_flag == 0){
printf ("Turning flag: 0\n");
if (is_wall_right == 1) count_right ++;
//Check if wall on right detected MAX_COUNT times to make sure we moved out of the corner we just turned
if (count_right >= MAX_COUNT){
count_right = 0;
turning_flag = -1;
i=0;
while (i <= NUM_STATES){
state[i] = 0;
i++;
}
i = -1;
}
else
return 0;
}
///Check if walls exists and increment the corresponding state
if (is_wall_right == 1 && is_wall_front == 0 && is_wall_left == 1){ //case 0 - move Forward
state[0]++;
i = 0;
}
else if (is_wall_right == 1 && is_wall_front == 0 && is_wall_left == 0){ //case 1 - move Forward, mark sub-hallway left
state[1]++;
i = 1;
}
else if (is_wall_right == 1 && is_wall_front == 1 && is_wall_left == 1){ //case 2 - Turn Left, track yaw
state[2]++;
i = 2;
}
else if (is_wall_right == 1 && is_wall_front == 1 && is_wall_left == 0){ //case 3 - Turn Left
state[3]++;
i = 3;
}
else if (is_wall_right == 0 && is_wall_front == 1 && is_wall_left == 0){ //case 4 - Turn Right, mark sub-hallway left, track yaw
state[4]++;
i = 4;
}
else if (is_wall_right == 0 && is_wall_front == 1 && is_wall_left == 1){ //case 5 - Turn Right, track yaw
state[5]++;
i = 5;
}
else if (is_wall_right == 0 && is_wall_front == 0 && is_wall_left == 0){ //case 6 - Turn Right, mark subhallway left and ahead, track yaw
state[6]++;
i = 6;
}
else if (is_wall_right == 0 && is_wall_front == 0 && is_wall_left == 1){ //case 7 - Turn Right, mark subhallway ahead, track yaw
state[7]++;
i = 7;
}
///If a state has been incremented
if (i >= 0){
///If needed, ceil the incremented state to MAX_COUNT
if (state[i] > MAX_COUNT)
state[i] = MAX_COUNT;
///Decrement the other states by 1
int k;
for(k = 0; k < NUM_STATES; k++){
if (k != i){
state[k]--;
state[k] = maximum(state[k],0);
}
}
}
else{
printf ("Error - Check_Environment() called but no state updated");
return 0;
}
return i; //Return the state that was incremented
}
task main() {
RobotState state;
initialize(&state);
short leftSpeed, rightSpeed;
short prevState = INITIALSTATE;
bool distLess50;
bool irDetected = false;
bool sawRedBlue = false;
bool goBackward = false;
float startAngle = 0;
//waitForStart();
wait1Msec(state.delayTime*1000);
INITIALDRIVE();
while(true){
//if state changes: stop motors, play tone, reset timers, gyro and lights
if (prevState != state.currentState){
if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
stopMotors();
leftSpeed = 0;
rightSpeed = 0;
}
AUDIO_DEBUG(500, 10);
time1[T1] = 0;
resetGyroAccel(&state);
LEDController(0x00);
distLess50 = false;
startAngle = state.degrees;
}
getSensors(&state);
prevState = state.currentState;
if(state.currentState == FINDLINE_TURN){
drive(0, TURNSPEED);
if(state.color2 == RED || state.color2 == BLUE){
sawRedBlue = true;
}
if (sawRedBlue && state.color2 == BLACK){
state.currentState = LINEFOLLOW;
}
if(abs(state.degrees) > 10){
state.currentState = LINEFOLLOW;
}
/* state FINDLINE_DRIVE seems unnecessary
} else if (state.currentState == FINDLINE_DRIVE) {
drive(DRIVESPEED, DRIVESPEED*.95);
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if(state.color2 == RED || state.color2 == BLUE){
state.currentState = LINEFOLLOW;
} */
} else if (state.currentState == LINEFOLLOW) {
if (state.dist < 50) {
distLess50 = true;
}
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if (state.dist > 50 && distLess50 && irDetected == true) {
state.currentState = GOTOEND;
} else if (goBackward){
if (state.color == RED || state.color == BLUE) {
leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
rightSpeed = -DRIVESPEED;
}else {
leftSpeed = -DRIVESPEED;
rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
}
} else {
if (state.color2 == RED || state.color2 == BLUE) {
leftSpeed = DRIVESPEED*LINEFOLLOWRATIO;
rightSpeed = DRIVESPEED;
} else {
leftSpeed = DRIVESPEED;
rightSpeed = DRIVESPEED*LINEFOLLOWRATIO;
}
}
drive(leftSpeed, rightSpeed);
} else if (state.currentState == SCOREBLOCK) {
irDetected = true;
goBackward = true;
LEDController(B2);
blockScorer();
state.currentState = LINEFOLLOW;
} else if (state.currentState == GOTOEND) {
drive(-DRIVESPEED, -DRIVESPEED);
if(time1[T1] >= 300) {
state.currentState = PARK_TURN1;
}
} else if (state.currentState == PARK_TURN1) {
drive(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 15){
state.currentState = PARK_DRIVE1;
}
} else if (state.currentState == PARK_DRIVE1) {
drive(-DRIVESPEED, -DRIVESPEED);
if(state.color == RED || state.color == BLUE){
state.currentState = PARK_TURN2;
}
} else if (state.currentState == PARK_TURN2) {
DRIVESPECIAL(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 22.5){
state.currentState = PARK_DRIVE2; //skip state HARVEST
}
} else if (state.currentState == HARVEST) {
motor[harvester] = 100;
DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
if (time1[T1] >= 500){
leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
drive(leftSpeed, rightSpeed);
if (abs(startAngle-state.degrees) <= 0.5){
state.currentState = PARK_DRIVE2;
}
}
} else if (state.currentState == PARK_DRIVE2) {
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(abs(state.x_accel) > 35 && state.x_accel < 100){
wait1Msec(20);
state.currentState = CHECKEND;
}
} else if (state.currentState == CHECKEND) {
if(abs(state.x_accel) > 35 && abs(state.x_accel) < 100){
state.currentState = END; //if it's > 30 after 1 sec of pushing, you're done
} else {
state.currentState = PARK_DRIVE2; //else you need to do more pushing
}
} else if (state.currentState == END){
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(time1[T1] >= 1000){
break;
}
} else {
break;
}
}
}
bool QS::BehaviorDefinition::operator==(const BehaviorDefinition &theOther)
const noexcept
{
return (getName() == theOther.getName() &&
getSensors() == theOther.getSensors());
}
/**
* Process OI Op Codes passed by controller
*/
void OpenInterface::handleOpCode(byte oc)
{
#ifdef DEBUG_SERIAL
switch (oc)
{
case ('s'):
oc = OC_SAFE;
break;
case ('d'):
oc = OC_DIRECT_DRIVE;
break;
case ('g'):
oc = OC_SENSORS;
break;
}
#endif
switch (oc)
{
case(OC_LOW_SIDE_DRIVERS):
callCallbackWithOneByte(controlLsdOutputCallback);
break;
case(OC_LEDS):
callCallbackWithThreeBytes(controlLedsCallback);
break;
case(OC_PWM_LOW_SIDE_DRIVERS):
callCallbackWithThreeBytes(controlLsdPwmCallback);
break;
case(OC_DIGITAL_OUTPUTS):
callCallbackWithOneByte(controlDigitalOutputCallback);
break;
case(OC_STREAM):
stream();
break;
case(OC_PAUSE_RESUME_STREAM):
break;
case(OC_SEND_IR):
callCallbackWithOneByte(sendIrCallback);
break;
case(OC_SHOW_SCRIPT):
showScript();
break;
case(OC_WAIT_EVENT):
callCallbackWithOneByte(waitEventCallback);
break;
case(OC_PLAY_SCRIPT):
scriptPlay();
break;
case(OC_SCRIPT):
scriptSet();
break;
case(OC_WAIT_ANGLE):
if (waitAngleCallback)
{
waitAction(waitAngleCallback);
}
break;
case(OC_WAIT_DISTANCE):
if (waitDistanceCallback)
{
waitAction(waitDistanceCallback);
}
break;
case(OC_WAIT_TIME):
waitTime();
break;
case(OC_DEMO):
case(OC_SPOT):
case(OC_COVER):
case(OC_COVER_AND_DOCK):
// These are all demo codes. Implement?
break;
case(OC_QUERY_LIST):
queryList();
break;
case (OC_START):
sensor[OI_SENSOR_OI_MODE] = OI_MODE_PASSIVE;
break;
case (OC_BAUD):
break;
case (OC_CONTROL):
case (OC_SAFE):
sensor[OI_SENSOR_OI_MODE] = OI_MODE_SAFE;
break;
case (OC_SENSORS):
getSensors();
break;
case (OC_SONG):
song();
break;
case(OC_PLAY_SONG):
songPlay();
break;
case (OC_DIRECT_DRIVE):
driveDirect();
break;
case (OC_DRIVE):
drive();
break;
case (OC_FULL):
sensor[OI_SENSOR_OI_MODE] = OI_MODE_FULL;
break;
}
}
task main() {
RobotState state;
initialize(&state);
short leftSpeed, rightSpeed;
short prevState = INITIALSTATE;
bool distLess50;
bool irDetected = false;
bool goBackward = false;
float startAngle = 0;
int numTimeAccelTriggered = 0;
waitForStart();
wait1Msec(state.delayTime*1000);
INITIALDRIVE();
while(true){
//if state changes: stop motors, play tone, reset timers, gyro and lights
if (prevState != state.currentState){
if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
stopMotors();
leftSpeed = 0;
rightSpeed = 0;
}
AUDIO_DEBUG(500, 10);
time1[T1] = 0;
resetGyroAccel(&state);
LEDController(0x00);
distLess50 = false;
startAngle = state.degrees;
}
getSensors(&state);
prevState = state.currentState;
if(state.currentState == FINDLINE_TURN){
drive(0, TURNSPEED);
if(abs(state.degrees) > 10){
state.currentState = LINEFOLLOW;
}
} else if (state.currentState == LINEFOLLOW) {
if (state.dist < 50) {
distLess50 = true;
}
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if (state.dist > 50 && distLess50 && irDetected == true){
state.currentState = GOTOEND;
} else if (goBackward){
leftSpeed = -DRIVESPEED;
rightSpeed = -DRIVESPEED;
} else {
leftSpeed = DRIVESPEED;
rightSpeed = DRIVESPEED;
}
drive(leftSpeed, rightSpeed);
} else if (state.currentState == SCOREBLOCK) {
irDetected = true;
goBackward = true;
LEDController(B2);
blockScorer();
state.currentState = LINEFOLLOW;
} else if (state.currentState == GOTOEND) {
drive(-DRIVESPEED, -DRIVESPEED);
if(time1[T1] >= 300) {
state.currentState = PARK_TURN1;
}
} else if (state.currentState == PARK_TURN1) {
drive(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 15){
state.currentState = PARK_DRIVE1;
}
} else if (state.currentState == PARK_DRIVE1) {
drive(-DRIVESPEED, -DRIVESPEED);
if((state.color == RED || state.color == BLUE) && time1[T1] > 500){
state.currentState = PARK_TURN2;
}
} else if (state.currentState == PARK_TURN2) {
DRIVESPECIAL(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 27){
state.currentState = PARK_DRIVE2; //skip state HARVEST
}
} else if (state.currentState == HARVEST) {
motor[harvester] = 100;
DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
if (time1[T1] >= 500){
leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
drive(leftSpeed, rightSpeed);
if (abs(startAngle-state.degrees) <= 0.5){
state.currentState = PARK_DRIVE2;
}
}
} else if (state.currentState == PARK_DRIVE2) {
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(abs(state.x_accel) > 35 && time1[T1] >= 300){
numTimeAccelTriggered++;
} else {
numTimeAccelTriggered = 0;
}
if (numTimeAccelTriggered >= 3) {
state.currentState = END;
}
wait1Msec(20);
} else if (state.currentState == END){
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(time1[T1] >= 300){
break;
}
} else {
break;
}
}
}
