// Main program.
int main(int argc, char *argv[]) {
// Command line parsing
commandline_init();
commandline_option_register("-b", "--binary", cCommandLine_Option);
commandline_parse(argc, argv);
// Help
if (commandline_option_provided("-h", "--help")) {
help();
exit(1);
}
// Initialization
algorithm_init();
// Run
algorithm_run();
return 0;
}
// Main program.
int main(int argc, char *argv[]) {
// Command line parsing
commandline_init();
commandline_option_register("-l", "--only-left", cCommandLine_Option);
commandline_option_register("-r", "--only-right", cCommandLine_Option);
commandline_parse(argc, argv);
// Help
if (commandline_option_provided("-h", "--help")) {
help();
exit(1);
}
// Initialization
khepera3_init();
// Initialize left motor
if (! commandline_option_provided("-r", "--only-right")) {
if (khepera3_motor_initialize(&(khepera3.motor_left))) {
printf("Left motor initialized.\n");
} else {
printf("Left motor: initialization failed.\n");
}
}
// Initialize right motor
if (! commandline_option_provided("-l", "--only-left")) {
if (khepera3_motor_initialize(&(khepera3.motor_right))) {
printf("Right motor initialized.\n");
} else {
printf("Right motor: initialization failed.\n");
}
}
return 0;
}
int main(int argc, char **argv) {
GOptionContext *context;
RoccatDevice *konextd;
GError *local_error = NULL;
int retval = EXIT_SUCCESS;
KoneplusRmp *rmp = NULL;
roccat_secure();
roccat_textdomain();
context = commandline_parse(&argc, &argv);
if (parameter_just_print_version) {
g_print(VERSION_STRING "\n");
goto exit1;
}
#if !(GLIB_CHECK_VERSION(2, 36, 0))
g_type_init();
#endif
konextd = konextd_device_first();
if (konextd == NULL) {
g_critical(_("No %s found."), KONEXTD_DEVICE_NAME);
retval = EXIT_FAILURE;
goto exit1;
}
if (parameter_just_activate_driver_state) {
if (!koneplus_device_state_write(konextd, KONEPLUS_DEVICE_STATE_STATE_ON, &local_error)) {
g_critical(_("Could not activate driver state: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_just_deactivate_driver_state) {
if (!koneplus_device_state_write(konextd, KONEPLUS_DEVICE_STATE_STATE_OFF, &local_error)) {
g_critical(_("Could not deactivate driver state: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_just_update_firmware) {
if (!update_firmware(konextd, parameter_just_update_firmware, &local_error)) {
g_critical(_("Could not update firmware: %s"), local_error->message);
retval = EXIT_FAILURE;
} else
g_message(_("Firmware updated successfully. Please reconnect device."));
goto exit2;
}
if (parameter_just_print_actual_profile) {
if (!print_actual_profile(konextd, &local_error)) {
g_critical(_("Could not print actual profile: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_just_reset) {
if (!reset(konextd, &local_error)) {
g_critical(_("Could not reset device: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_read_firmware) {
if (!print_firmware(konextd, &local_error)) {
g_critical(_("Could not print firmware version: %s"), local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
}
if (parameter_sensor_read) {
if (!print_sensor_value(konextd, &local_error)) {
g_critical(_("Could not read sensor register: %s"), local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
}
if (parameter_sensor_write != -1) {
koneplus_sensor_write_register(konextd, parameter_sensor_register, parameter_sensor_write, &local_error);
if (local_error) {
g_critical(_("Could not write sensor register: %s"), local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
}
if (parameter_load != -1) {
rmp = konextd_rmp_load(konextd, parameter_load - 1, &local_error);
if (!rmp) {
g_critical(_("Could not load profile %i: %s"), parameter_load, local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
} else if (parameter_in_rmp) {
rmp = koneplus_rmp_read_with_path(parameter_in_rmp, konextd_rmp_defaults(), &local_error);
if (!rmp) {
g_critical(_("Could not read rmp %s: %s"), parameter_in_rmp, local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
} else
rmp = konextd_default_rmp();
koneplus_rmp_set_modified(rmp);
if (parameter_save != -1) {
if (!konextd_rmp_save(konextd, rmp, parameter_save - 1, &local_error)) {
g_critical(_("Could not save profile %i: %s"), parameter_save, local_error->message);
retval = EXIT_FAILURE;
goto exit3;
}
konextd_dbus_emit_profile_data_changed_outside_instant(parameter_save);
}
if (parameter_out_rmp) {
if (!koneplus_rmp_write_with_path(parameter_out_rmp, rmp, &local_error)) {
g_critical(_("Could not write rmp %s: %s"), parameter_out_rmp, local_error->message);
retval = EXIT_FAILURE;
goto exit3;
}
}
if (parameter_activate_profile != -1) {
if (!koneplus_actual_profile_write(konextd, parameter_activate_profile -1, &local_error)) {
g_critical(_("Could not activate profile %i: %s"), parameter_activate_profile, local_error->message);
retval = EXIT_FAILURE;
goto exit3;
}
konextd_dbus_emit_profile_changed_outside_instant(parameter_activate_profile);
}
exit3:
koneplus_rmp_free(rmp);
exit2:
g_object_unref(G_OBJECT(konextd));
exit1:
commandline_free(context);
g_clear_error(&local_error);
exit(retval);
}
int main(int argc, char **argv) {
GOptionContext *context;
RoccatDevice *nyth;
GError *local_error = NULL;
int retval = EXIT_SUCCESS;
roccat_secure();
roccat_textdomain();
context = commandline_parse(&argc, &argv);
if (parameter_just_print_version) {
g_print(VERSION_STRING "\n");
goto exit1;
}
#if !(GLIB_CHECK_VERSION(2, 36, 0))
g_type_init();
#endif
nyth = nyth_device_first();
if (nyth == NULL) {
g_critical(_("No %s found."), NYTH_DEVICE_NAME_COMBINED);
retval = EXIT_FAILURE;
goto exit1;
}
if (parameter_just_activate_driver_state) {
if (!nyth_device_state_write(nyth, NYTH_DEVICE_STATE_STATE_ON, &local_error)) {
g_critical(_("Could not activate driver state: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_just_deactivate_driver_state) {
if (!nyth_device_state_write(nyth, NYTH_DEVICE_STATE_STATE_OFF, &local_error)) {
g_critical(_("Could not deactivate driver state: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_just_print_actual_profile) {
if (!print_actual_profile(nyth, &local_error)) {
g_critical(_("Could not print actual profile: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_just_reset) {
if (!reset(nyth, &local_error)) {
g_critical(_("Could not reset device: %s"), local_error->message);
retval = EXIT_FAILURE;
}
goto exit2;
}
if (parameter_read_firmware) {
if (!print_firmware(nyth, &local_error)) {
g_critical(_("Could not print firmware version: %s"), local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
}
if (parameter_activate_profile != -1) {
if (!nyth_profile_write_index(nyth, parameter_activate_profile - 1, &local_error)) {
g_critical(_("Could not activate profile %i: %s"), parameter_activate_profile, local_error->message);
retval = EXIT_FAILURE;
goto exit2;
}
nyth_dbus_emit_profile_changed_outside_instant(parameter_activate_profile - 1);
}
exit2:
g_object_unref(G_OBJECT(nyth));
exit1:
commandline_free(context);
g_clear_error(&local_error);
exit(retval);
}
int main(int argc, char *argv[])
{
odometry_track_init();
khepera3_init();
commandline_init();
commandline_parse(argc,(char *)argv);
if(commandline_option_provided("-h","--help"))
{
printf("*******************************\n* Tracker1\n* Ryan G. Hunter\n* Boston Universtiy Intelligent Mechatronics Lab\n* \n* Parameters:\n*\n* PD Gains Settings:\n* Distance -d --distance Pixel Distance Rho\n* Rho Gain -kRo --kRo Distance Proportional gain\n* Omega Gain -kOm --kOmega Phi (omega) Proportional Gain\n* Omega -Om --omega Set Omega Constant\n*********************************\n");
return 0;
}
struct control options;
options.distance = commandline_option_value_int("-d", "--distance", 10); //TODO:NEED TO COME UP WITH DEFAULT
options.kRop = commandline_option_value_float("-kRop","--kRop",75);
options.kRod = commandline_option_value_float("-kRod","--kRod",8);
options.kOmegaXp = commandline_option_value_float("-kOmXp","--kOmegaXp",20);
options.kOmegaXd = commandline_option_value_float("-kOmXd","--kOmegaXd",3);
double turn_radius=(double)commandline_option_value_float("-tRad","--TurnRadius",1.0);
double txer_reference_x=(double)commandline_option_value_float("-txRef","-txrReferenceX",0.0);
khepera3_motor_initialize(&khepera3.motor_left);
khepera3_motor_initialize(&khepera3.motor_right);
khepera3_motor_start(&khepera3.motor_left);
khepera3_motor_start(&khepera3.motor_right);
int gps_sock, com_sock;
if(!initSocket(&gps_sock,4950))
printf("issue with socket\n");
if(!initSocket(&com_sock,4100))
printf("Problem with comm sock\n");
currentVehicle.id=VEHICLE_ID;
enum state CurrState = STOP, CurrLaw = VSPY;
struct robot xmit,rcvr,spy;
struct timeval epochTimer;
xmit.id = commandline_option_value_int("-idT", "--signalerID", 1);
rcvr.id = commandline_option_value_int("-idR", "--receiverID", 2);
spy.id = commandline_option_value_int("-idSpy", "--spyID", 0);
int role = commandline_option_value_int("-r", "--role", 1);
#ifdef DEBUG
printf("Distance: %i\nkRop: %f\nkRod: %f\nkOmegaXp: %f\nkOmegaXd: %f\nTransmitter ID: %i\nRecvr ID: %i\nSpy ID: %i\nRole: %i\n",options.distance,options.kRop,options.kRod,options.kOmegaXp,options.kOmegaXd,xmit.id,rcvr.id,spy.id,role);
#endif
switch (role)
{
case 1:
CurrLaw = CAMOTRACK;
break;
case 2:
CurrLaw = RECEIVE;
break;
case 3:
CurrLaw = VSPY;
break;
}
#ifdef DEBUG
printf("CurrLaw: %i\n",CurrLaw);
#endif
struct packet buffer, commands;
int vel;
double oldVal = 0;
khepera3_drive_set_current_position(0,0);
float xErrorOld= 0;
char holdRun = 1;
while(1)
{
checkMessages(&gps_sock,&buffer);
checkMessages(&com_sock,&commands);
khepera3_drive_get_current_speed();
#ifdef OBAVOID
if(checkObst() && CurrState != HOLD)
CurrState=OBDETECTED;
#endif
checkStateChange(&commands,&CurrState);
#ifdef DEBUG
// printf("CurrState: %i CurrLaw: %i\n",CurrState,CurrLaw);
#endif
switch(CurrState)
{
case STOP:
khepera3_drive_set_speed(0,0);
CurrState=HOLD;
break;
case START:
khepera3_drive_set_speed(10000,10000);
CurrState = GETPOS;
holdRun = 1;
if(CurrLaw == RECEIVE)
captureFlag = SPY+XMIT;
if(CurrLaw == VSPY){
printf("ping\n");
captureFlag = XMIT;}
break;
case HOLD: //intended to be an intermediate state when waiting for a next state from the handheld/client
#ifdef DEBUG
if(holdRun){printf("On Hold\n"); holdRun=0;}
#endif
break;
case EXIT:
khepera3_drive_stop();
close(&gps_sock);
close(&com_sock);
printf("Exiting...\n");
return 0;
break;
#ifdef OBAVOID
case OBDETECTED:
khepera3_drive_set_current_position(0,0);
khepera3_drive_get_current_position();
long sensorBuff[11];
irProx(sensorBuff);
float leftAv=(sensorBuff[1]+sensorBuff[2]+sensorBuff[3])/3.0;
float rightAv=(sensorBuff[4]+sensorBuff[5]+sensorBuff[6])/3.0;
int turnVal = 2.11*2765 / 4;
if(leftAv>rightAv)
{
leftTurn = khepera3.motor_left.current_position-turnVal;
rightTurn = khepera3.motor_right.current_position+turnVal;
}
else if(leftAv<rightAv)
{
leftTurn = khepera3.motor_left.current_position+turnVal;
rightTurn = khepera3.motor_right.current_position-turnVal;
}
else
{
leftTurn = khepera3.motor_left.current_position-2*turnVal;
rightTurn = khepera3.motor_right.current_position+2*turnVal;
}
printf("Right Pos: %i Left Pos: %i\n",rightTurn,leftTurn);
khepera3_drive_goto_position_using_profile(leftTurn,rightTurn);
CurrState=TURNDELAY; //TODO: not correct Reinsertion
break;
case TURNDELAY:
khepera3_drive_get_current_speed();
if(khepera3.motor_left.current_speed < 100 && khepera3.motor_right.current_speed < 100)
{
khepera3_drive_set_speed(10000,10000);
CurrState = TRACK;
roOld = 0;
phiOld = 0;
break;
}
else
break;
#endif
case GETPOS:
{
if(updatePosition(&xmit,&buffer))
{
captureFlag |= XMIT;
#ifdef DEBUG
printf("Got XMIT\nCapture Flag: %i\n",captureFlag);
#endif
break;
}
if(updatePosition(&spy,&buffer))
{
captureFlag |= SPY;
#ifdef DEBUG
printf("Got SPY\nCapture Flag; %i\n",captureFlag);
#endif
break;
}
if(updatePosition(&rcvr,&buffer))
{
captureFlag |= RCVR;
#ifdef DEBUG
printf("Got RCVR\nCapture Flag %i\n",captureFlag);
#endif
break;
}
if(captureFlag == ALL)
{
#ifdef DEBUG
printf("Got ALL\nCapture Flag: %i\n",captureFlag);
#endif
CurrState = CurrLaw;
}
break;
}
case CAMOTRACK:
{
#ifdef DEBUG
printf("\nBegin Debug for Camotrack\n");
#endif
double x1 = (double)(xmit.x), x2=(double)(rcvr.x), x3=(double)(spy.x);
double y1 = (double)(xmit.y), y2=(double)(rcvr.y), y3=(double)(spy.y);
//create vector structs
struct vector v32,v21;
v32.i = x2-x3;
v32.j = y2-y3;
v21.i = x1-x2;
v21.j = y1-y2;
#ifdef DEBUG
printf("X1: %f Y1: %f\nX2: %f Y2: %f\nX3: %f Y3: %f\nV21 i: %f j: %f\nV32 i: %f j: %f\n",x1,y1,x2,y2,x3,y3,v21.i,v21.j,v32.i,v32.j);
#endif
//scale vectors
double v32Mag = vMag(&v32);
v32.i = v32.i/v32Mag;
v32.j = v32.j/v32Mag;
double v21Mag = vMag(&v21);
v21.i=v21.i/v21Mag;
v21.j = v21.j/v21Mag;
#ifdef DEBUG
printf("V21N i: %f j: %f Mag: %f\nV32N i: %f j: %f Mag: %f\n",v21.i,v21.j,v21Mag,v32.i,v32.j,v32Mag);
#endif
double z = v32.i*v21.j-v32.j*v21.i;
double proportion = z*options.kRop;
double deriv = options.kRod*(z-oldVal);
vel = (int)((oldVal == 0) ? proportion : (proportion+deriv));
//vel = (int)proportion;
#ifdef DEBUG
printf("Control Law Output\nz:%f\n",z);
#endif
#ifndef XTRACK
khepera3_drive_set_speed(vel,vel);
#endif
captureFlag = 0x00;
#ifdef DEBUG
printf("Vel: %i\n",vel);
#endif
#ifdef XTRACK
if(xmit.y > 150 && xmit.y < 450){CurrState=DLAW;}
else{CurrState = XLOCK;}
#else
CurrState = GETPOS;
#endif
oldVal = z;
break;
}
#ifdef XTRACK
case XLOCK:
{
#ifdef DEBUG
printf("\nDebug Data for X Lock\n");
#endif
int currX=0;
if(role == 1){
currX=xmit.x;}
if(role == 2){
currX=rcvr.x;}
if(role == 3){
currX=spy.x;}
if(firstRun){
txer_reference_x=currX;
CurrState=GETPOS;
firstRun=0;
break;}
float xError = (float)(currX-(int)txer_reference_x);
float omega = xErrorOld == 0 ? options.kOmegaXp*xError : options.kOmegaXp*xError+options.kOmegaXd*(xError-xErrorOld);
#ifdef DEBUG
//printf("Proportion: %f\nDerivative: %f\n",proportion,deriv);
#endif
khepera3_drive_set_speed((vel-omega),(vel+omega));
xErrorOld=xError;
CurrState = GETPOS;
#ifdef DEBUG
printf("Omega: %f\nxError: %f\n",omega,xError);
#endif
break;
}
#endif
case VSPY:
{
#ifdef DEBUG
printf("\nBegin VSPY Debug Info\n");
#endif
float y23 = (float)(rcvr.y-spy.y-options.distance);
float proportion = options.kRop*(y23);
if(oldVal !=0){vel = proportion + options.kRod*(oldVal - proportion);
}else{ vel = proportion;}
//khepera3_drive_set_speed(vel,vel);
if(proportion < 0)
proportion = 0;
khepera3_drive_set_speed((int)proportion,(int)proportion);
vel = proportion;
captureFlag = XMIT;
CurrState = XLOCK;
oldVal = vel;
#ifdef DEBUG
printf("Proportion: %f\nY-Distance: %f\n",proportion,y23);
#endif
break;
}
case RECEIVE:
{
vel = 10000;
#ifdef DEBUG
printf("properly Entered Recieve Loop\n");
#endif
CurrState = XLOCK;
captureFlag = XMIT+SPY;
break;
}
case DLAW:{
int leftv,rightv;
if(camoControl(&leftv,&rightv,&xmit,&rcvr,&spy,turn_radius,txer_reference_x,vel))
khepera3_drive_set_speed(leftv,rightv);
captureFlag = 0x00;
CurrState = GETPOS;
break;
}
}
}
return 0;
}
