int main ()
{
char message [512];
int n = 1;
int tps = 1;
if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0)
{
printf("[FAILED] Socket initialization failed\n");
}
else
{
sleep(1);
set_trim(message, n++);
usleep(500000);
while(tps < 100)
{
take_off(message, n++);
usleep(50000);
tps++;
}
tps = 0;
while(tps < 100)
{
reset_com(message);
usleep(50000);
tps++;
}
tps = 0;
landing(message, n++);
sleep(1);
}
close_socket();
return 0;
}
int main ()
{
char message [512];
int n = 1;
int tps = 1;
if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0)
{
printf("[FAILED] Socket initialization failed\n");
}
else
{
sleep(1);
set_trim(message, n++);
usleep(500000);
////////////// take off //////////////////////////
while(tps < 167) //100 avec 50ms
{
take_off(message, n++);
usleep(30000);
tps++;
}
tps = 0;
////////////// WAIT ////////////////////
while(tps < 133) //80
{
reset_com(message);
usleep(30000);
tps++;
}
tps = 0;
////////////// GO ////////////////////
while(tps < 58) //35
{
set_pitch(message, n++, FRONT, 0.25);
usleep(30000);
tps++;
}
tps = 0;
while(tps < 17) //10
{
set_pitch(message, n++, BACK, 0.25);
usleep(30000);
tps++;
}
tps = 0;
while(tps < 17)
{
set_pitch(message, n++, FRONT, 0.0);
usleep(30000);
tps++;
}
tps = 0;
////////////// WAIT ////////////////////
while(tps < 25)
{
reset_com(message);
usleep(30000);
tps++;
}
tps = 0;
////////////// ROTATE ////////////////////
while(tps < 110) //140
{
set_yaw(message, n++, LEFT, 0.4);
usleep(30000);
tps++;
}
tps = 0;
set_yaw(message, n++, RIGHT, 0.0);
usleep(30000);
////////////// WAIT ////////////////////
while(tps < 80)
{
reset_com(message);
usleep(30000);
tps++;
}
tps = 0;
////////////// RETURN ////////////////////
while(tps < 58)
{
set_pitch(message, n++, FRONT, 0.25);
usleep(30000);
tps++;
}
tps = 0;
while(tps < 17)
{
set_pitch(message, n++, BACK, 0.25);
usleep(30000);
tps++;
}
tps = 0;
while(tps < 17)
{
set_pitch(message, n++, FRONT, 0.0);
usleep(30000);
tps++;
}
tps = 0;
////////////// WAIT ////////////////////
while(tps < 167)
{
reset_com(message);
usleep(30000);
tps++;
}
tps = 0;
////////////// LAND ////////////////////
landing(message, n++);
sleep(1);
}
close_socket();
return 0;
}
int main ()
{
char message [512];
int n = 1;
int tps = 1;
int wait =1;
/*
//handle the ctrl -c to make the drone land
struct sigaction act;
memset(&act,0,sizeof(act));
act.sa_handler = intHandler;
sigaction(SIGINT, &act, NULL);
*/
if (init_socket() != 0)
{
printf("[FAILED] Socket initialization failed\n");
}
else //complex_move(...;float roll_power, float pitch_power, float vertical_power, float yaw_power)
{
sleep(1);
printf("demarrage\n");
set_trim(message, n++, wait);
while(tps < 167)
{
take_off(message, n++, wait);
tps++;
}
tps = 0;
//test de fonction
at_config(message, n++, "pic:ultrasound_freq","2");
//while(tps < 133)
//while(1)
while(keepRunning)
{
reset_com(message, wait);
tps++;
}
tps = 0;
/*
printf("debut commande\n");
while(tps < 500)
{
//go front and up and turning clockwise
set_complex_move(message,n++,0,-0.05,0.1,0.05,wait);
tps++;
}
tps = 0;
set_complex_move(message, n++,0,0,0,0,wait);
while(tps < 133)
{
reset_com(message,wait);
tps++;
}
tps = 0;
*/
landing(message, n++,wait);
sleep(1);
}
close(sockfd);
return 0;
}
//A GCode has been received
//See if the current Gcode line has some orders for us
void SCARAcal::on_gcode_received(void *argument)
{
Gcode *gcode = static_cast<Gcode *>(argument);
if( gcode->has_m) {
switch( gcode->m ) {
case 114: { // Extra stuff for Morgan calibration
char buf[32];
float cartesian[6],
actuators[6];
THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
int n = snprintf(buf, sizeof(buf), " A: Th:%1.3f Ps:%1.3f",
actuators[0],
actuators[1]); // display actuator angles Theta and Psi.
gcode->txt_after_ok.append(buf, n);
gcode->mark_as_taken();
}
return;
case 360: {
float target[2] = {0.0F, 120.0F},
S_trim[3];
this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values
if(gcode->has_letter('P')) {
// Program the current position as target
float cartesian[6],
actuators[6],
S_delta[2],
S_trim[3];
THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
S_delta[0] = actuators[0] - target[0];
set_trim(S_delta[0], S_trim[1], 0, gcode->stream);
} else {
set_trim(0, S_trim[1], 0, gcode->stream); // reset trim for calibration move
this->home(); // home
SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
}
gcode->mark_as_taken();
}
return;
case 361: {
float target[2] = {90.0F, 130.0F};
if(gcode->has_letter('P')) {
// Program the current position as target
float cartesian[6],
actuators[6];
THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate to get actuator position
STEPPER[0]->change_steps_per_mm(actuators[0] / target[0] * STEPPER[0]->get_steps_per_mm()); // Find angle difference
STEPPER[1]->change_steps_per_mm(STEPPER[0]->get_steps_per_mm()); // and change steps_per_mm to ensure correct steps per *angle*
} else {
this->home(); // home - This time leave trims as adjusted.
SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
}
gcode->mark_as_taken();
}
return;
case 364: {
float target[2] = {45.0F, 135.0F},
S_trim[3];
this->get_trim(S_trim[0], S_trim[1], S_trim[2]); // get current trim to conserve other calbration values
if(gcode->has_letter('P')) {
// Program the current position as target
float cartesian[6],
actuators[6],
S_delta[2];
THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
THEKERNEL->robot->arm_solution->cartesian_to_actuator( cartesian, actuators ); // translate it to get actual actuator angles
S_delta[1] = actuators[1] - target[1]; // Find difference, and
set_trim(S_trim[0], S_delta[1], 0, gcode->stream); // set trim to reflect the difference
} else {
set_trim(S_trim[0], 0, 0, gcode->stream); // reset trim for calibration move
this->home(); // home
SCARA_ang_move(target[0], target[1], 100.0F, slow_rate * 3.0F); // move to target
}
gcode->mark_as_taken();
}
return;
}
}
}
void SWITCH_DRONE_COMMANDE(int _order)
{
char message [64];
switch(_order){
case 0 :
reset_com(message);
break;
case 1 :
landing(message);
inC.flag_land = 0;
break;
case 2 :
//printf("SWITCH COMMANDE take off\n");
take_off(message);
inC.flag_takeoff = 0;
break;
case 3 :
//printf("case 3 : set_roll, roll_power=%f\n", roll_power);
set_roll(message, roll_move, roll_power);
inC.flag_rollcalled = 0;
break;
case 4 :
//printf("case 4 : set_pitch, pitch_power=%f, yaw_power=%f\n", pitch_power, yaw_power);
set_pitch(message, pitch_move, pitch_power);
inC.flag_pitchcalled = 0;
break;
case 5 :
set_yaw(message, yaw_move, yaw_power);
inC.flag_yawcalled = 0;
break;
case 6 :
set_roll(message, roll_move, roll_power);
set_pitch(message, pitch_move, pitch_power);
inC.flag_rollpitchcalled = 0;
break;
case 7 :
set_trim(message);
inC.flag_calibH = 0;
break;
case 8 :
calibrate_magneto(message);
sleep(3);
while(1)
{
set_yaw(message, RIGHT, 0.1);
while(isControllerReady()==0);
Main_Nav = getNavdata();
if (Main_Nav.magneto.heading_fusion_unwrapped > 0.0)
nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped - 360.0;
else
nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped + 360.0;
printf("angle_trouve et angle +/- 360 = %.2f, %.2f \r", Main_Nav.magneto.heading_fusion_unwrapped, nav_suiv);
if(nav_suiv < 0.0 && nav_prec > 0.0 || nav_suiv > 0.0 && nav_prec < 0.0)
{
inC.flag_calibM = 0;
printf("\nnav_suiv = %.2f | nav_prec = %.2f\n", nav_suiv, nav_prec);
break;
}
nav_prec = nav_suiv;
}
set_yaw(message, LEFT, 0.0);
inC.flag_calibM = 0;
break;
case 9 :
//set_emergency(message);
inC.flag_emergency = 0;
set_gaz(message, UP, 0.2);// this is an ugly hack (don't commit this)
break;
case 10 :
//anti_emergency(message);
inC.falg_antiemergency = 0;
set_gaz(message, UP, 0.0);// this is an ugly hack (don't commit this)
break;
case 20 :
calcul_mission();
break;
default :
printf("Scade ne marche pas si bien que ça finalement\n");
break;
}
}
/**
* @brief function designed to be the main of a thread
* Sends movement commands to the drone in order to follow the beacon
*/
void * track_position(void * arg){
// check time interval
struct timeval old_tv = {0};
struct timeval tv = {0};
long unsigned int elapsed_time = 0; // in microsecondss
// moves
char message [512];
int tps = 1;
int wait =1;
//handle the ctrl -c to make the drone land
struct sigaction act;
memset(&act,0,sizeof(act));
act.sa_handler = intHandlerThread3;
sigaction(SIGINT, &act, NULL);
gettimeofday(&old_tv, NULL);
if (init_socket() != 0)
{
printf("[FAILED] Socket initialization failed\n");
}
else
{
//////////////////////////////////////////////////////////
// TAKING OFF
//////////////////////////////////////////////////////////
sleep(1);
printf("Drone starts flying...\n");
set_trim(message, wait);
printf("Taking off...\n");
while(tps < 167)
{
take_off(message, wait);
tps++;
}
//stop waiting 40 us after a command send
wait = 0;
// Go up to be at shoulder level
printf("Going up...\n");
gettimeofday(&old_tv, NULL);
elapsed_time = 0;
while(elapsed_time < 2)
{
set_simple_move(message, UP, 1, wait);
gettimeofday(&tv, NULL);
elapsed_time = (tv.tv_sec-old_tv.tv_sec);
}
set_simple_move(message, UP, 0.0, wait);
elapsed_time = 35000;
while(keepRunning){
while (elapsed_time < 35000)
{
gettimeofday(&tv, NULL);
elapsed_time = (tv.tv_sec-old_tv.tv_sec)*1000000 + (tv.tv_usec-old_tv.tv_usec);
}
pthread_mutex_lock(&track_pos_mux);
print_position();
///////////////////////////////////////////////////////////////////////
// MOVES TO HAVE THE RIGHT ANGLE AND RIGHT DISTANCE FROM THE EMIITER
///////////////////////////////////////////////////////////////////////
reset_com(message, wait);
//If no signal has been detected
if(!pos.signalDetected)
{
// stop moving
set_simple_move(message, FRONT, 0, wait);
}
// If a signal has been detected, move !
else
{
if(pos.angle >= -ANGLE_PRECISION/2 && pos.angle <= ANGLE_PRECISION/2)
{
// For now, always move forward when the source in front of the drone
set_simple_move(message, FRONT, 0.05, wait);
// And now manage distance
// if(pos.distance > 200) // in cm
// set_simple_move(message, FRONT, 0.05, wait);
// else if(pos.distance < 180) // in cm
// set_simple_move(message, BACK, 0.05, wait);
// else
// set_simple_move(message, FRONT, 0, wait);
}
else if (pos.angle > ANGLE_PRECISION/2)
set_simple_move(message, CLKWISE, 0.5, wait);
else
set_simple_move(message, ANTI_CLKWISE, 0.5,wait);
}
pthread_mutex_unlock(&compute_pos_mux);
gettimeofday(&old_tv, NULL);
elapsed_time=0;
}
///////////////////////////////////////////
// LANDING
///////////////////////////////////////////
landing(message, wait);
sleep(1);
}
pthread_exit(NULL);
}
int main ()
{
char message [512];
int n = 1;
int tps = 1;
if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0)
{
printf("[FAILED] Socket initialization failed\n");
}
else
{
sleep(1);
set_trim(message, n++);
usleep(500000);
while(tps < 167)
{
take_off(message, n++);
usleep(30000);
tps++;
}
tps = 0;
while(tps < 133)
{
reset_com(message);
usleep(30000);
tps++;
}
tps = 0;
/* while(tps < 30)
{
set_pitch(message, n++, BACK, 0.25);
usleep(50000);
tps++;
}
tps = 0;
while(tps < 10)
{
set_pitch(message, n++, FRONT, 0.1);
usleep(50000);
tps++;
}
tps = 0;
while(tps < 30)
{
set_pitch(message, n++, BACK, 0.0);
usleep(50000);
tps++;
}
tps = 0;*/
/* while(tps < 60)
{
take_off(message, n++);
usleep(50000);
tps++;
}
tps = 0;*/
while(tps < 150)
{
set_yaw(message, n++, LEFT, 0.4);
usleep(30000);
tps++;
}
tps = 0;
set_yaw(message, n++, RIGHT, 0.0);
usleep(30000);
while(tps < 133)
{
reset_com(message);
usleep(30000);
tps++;
}
tps = 0;
landing(message, n++);
sleep(1);
}
close_socket();
return 0;
}
bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode)
{
float target = 0.03F;
if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
bool keep = false;
if(gcode->has_letter('K')) keep = true; // keep current settings
gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius);
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F;
if(!keep) {
// zero trim values
if(!set_trim(0, 0, 0, gcode->stream)) return false;
} else {
// get current trim, and continue from that
if (get_trim(trimx, trimy, trimz)) {
gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz);
} else {
gcode->stream->printf("Could not get current trim, are endstops enabled?\n");
return false;
}
}
// find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
// so we need to find the initial place that the probe triggers when it hits the bed
float bedht= findBed();
if(isnan(bedht)) return false;
gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
// move to start position
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// get initial probes
// probe the base of the X tower
int s;
if(!zprobe->doProbeAt(s, t1x, t1y)) return false;
float t1z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T1-0 Z:%1.4f C:%d\n", t1z, s);
// probe the base of the Y tower
if(!zprobe->doProbeAt(s, t2x, t2y)) return false;
float t2z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T2-0 Z:%1.4f C:%d\n", t2z, s);
// probe the base of the Z tower
if(!zprobe->doProbeAt(s, t3x, t3y)) return false;
float t3z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T3-0 Z:%1.4f C:%d\n", t3z, s);
float trimscale = 1.2522F; // empirically determined
auto mm = std::minmax({t1z, t2z, t3z});
if((mm.second - mm.first) <= target) {
gcode->stream->printf("trim already set within required parameters: delta %f\n", mm.second - mm.first);
return true;
}
// set trims to worst case so we always have a negative trim
trimx += (mm.first - t1z) * trimscale;
trimy += (mm.first - t2z) * trimscale;
trimz += (mm.first - t3z) * trimscale;
for (int i = 1; i <= 10; ++i) {
// set trim
if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false;
// home and move probe to start position just above the bed
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// probe the base of the X tower
if(!zprobe->doProbeAt(s, t1x, t1y)) return false;
t1z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T1-%d Z:%1.4f C:%d\n", i, t1z, s);
// probe the base of the Y tower
if(!zprobe->doProbeAt(s, t2x, t2y)) return false;
t2z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T2-%d Z:%1.4f C:%d\n", i, t2z, s);
// probe the base of the Z tower
if(!zprobe->doProbeAt(s, t3x, t3y)) return false;
t3z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T3-%d Z:%1.4f C:%d\n", i, t3z, s);
mm = std::minmax({t1z, t2z, t3z});
if((mm.second - mm.first) <= target) {
gcode->stream->printf("trim set to within required parameters: delta %f\n", mm.second - mm.first);
break;
}
// set new trim values based on min difference
trimx += (mm.first - t1z) * trimscale;
trimy += (mm.first - t2z) * trimscale;
trimz += (mm.first - t3z) * trimscale;
// flush the output
THEKERNEL->call_event(ON_IDLE);
}
if((mm.second - mm.first) > target) {
gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mm.second - mm.first);
}
return true;
}
