# -*- makefile -*-
-SUBDIRS = contrib eb2008
+SUBDIRS = contrib
+
+bin_PROGRAMS += cand
+cand_SOURCES = cand.cc
+cand_LIBS = pthread sharp roboorte roboorte_generic robottype robottype orte
#include <can_msg_def.h>
#include <sharp.h>
#include <orte.h>
-#include <roboorte_eb2008.h>
+#include <roboorte.h>
#include <roboorte_generic.h>
#include <can_msg_masks.h>
-#include "cand_eb2008.h"
+#include "cand.h"
int cand_init()
{
+++ /dev/null
-# -*- makefile -*-
-
-bin_PROGRAMS += cand_eb2007
-cand_eb2007_SOURCES = cand_eb2007.cc
-cand_eb2007_LIBS = pthread sharp roboorte_eb2007 robottype robottype_eb2007 orte
+++ /dev/null
-/*
- * cand_eb2007.cc 08/04/07
- *
- * CAN daemon for the Eurobot 2007 competition.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#ifdef HAVE_CONFIG_H
-#include <config.h>
-#endif
-
-#include <iostream>
-#include <cstdlib>
-#include <sys/socket.h>
-#include <sys/ioctl.h>
-#include <fcntl.h>
-
-#include <af_can.h>
-#include <canutils.h>
-
-#include <can_ids.h>
-#include <sharp.h>
-#include <orte.h>
-#include <roboorte_eb2007.h>
-
-#include "cand_eb2007.h"
-
-int cand_init()
-{
- if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
- perror("cand: unable to create a socket");
- return -1;
- }
-
- can_addr.can_family = AF_CAN;
- strcpy(can_ifreq.ifr_name, "can0");
-
- if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
- perror("cand: SIOCGIFINDEX");
- return -2;
- }
-
- can_addr.can_ifindex = can_ifreq.ifr_ifindex;
-
- if (!can_addr.can_ifindex) {
- perror("cand: invalid socket interface");
- return -3;
- }
-
- if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
- perror("cand: unable to bind");
- return -4;
- }
-
- return 0;
-}
-
-int set_motor_speed(struct eb2007_orte_data *orte_data)
-{
- unsigned char data[4];
-
- data[0] = orte_data->motion_speed.left >> 8;
- data[1] = orte_data->motion_speed.left & 0xff;
- data[2] = orte_data->motion_speed.right >> 8;
- data[3] = orte_data->motion_speed.right & 0xff;
- can_send(CAN_MOTION_CMD, 4, data);
-
- /*printf("data: ");
- for (int i=0; i< 4; i++) {
- printf("%02x ",data[i]);
- }
- printf("\n");*/
-
- return 0;
-}
-
-int set_serva(struct eb2007_orte_data *orte_data)
-{
- unsigned char data[6];
-
- data[5] = orte_data->servos.backDoor;
- data[1] = orte_data->servos.frontDoor;
- data[2] = orte_data->servos.transporterFront;
- data[3] = orte_data->servos.transporterInner;
- data[4] = orte_data->servos.innerDoor;
- data[0] = orte_data->servos.release;
- can_send(CAN_SERVO, 6, data);
-
- /*for(int i=0; i<6; i++)
- printf("data %d = 0x%x\n", i, data[i]);*/
-
- return 0;
-}
-
-int can_send(canid_t id, unsigned char length, unsigned char *data)
-{
- struct can_frame frame;
- int size;
-
- frame.can_id = id;
- frame.can_dlc = length;
-
- for(int i=0; i<length; i++)
- frame.data[i] = data[i];
-
- if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
- perror("cand: can send: write()");
- return -1;
- } else if (size < sizeof(struct can_frame)) {
- perror("cand: can send: incomplete CAN frame\n");
- return 1;
- }
-
- /*
- printf("write returned %d\n", size);
- printf("finnished can send\n");
- */
-
- return 0;
-}
-
-/**
- * Parse frame ID and react as required
- */
-int cand_parse_frame(struct eb2007_orte_data *orte, struct can_frame frame)
-{
- int status_cnt = 0;
- int adc1_cnt = 0;
- int adc2_cnt = 0;
- int adc3_cnt = 0;
- int ir_cnt = 0;
- unsigned short *las_meas;
- int las_mi, las_di, las_bcnt;
- int i;
-
- switch(frame.can_id) {
- /* positioning by odometry */
- case CAN_MOTION_ODOMETRY_SIMPLE:
- orte->motion_position.left =
- ((frame.data[0]<<24)|(frame.data[1]<<16)|
- (frame.data[2]<<8)|(frame.data[3]));
- orte->motion_position.right =
- ((frame.data[4]<<24)|(frame.data[5]<<16)|
- (frame.data[6]<<8)|(frame.data[7]));
- ORTEPublicationSend(orte->publicationMotionPos);
- break;
-
- /* motion status */
- case CAN_MOTION_STATUS:
- orte->motion_status.err_left =
- (frame.data[0]<<8)|(frame.data[1]);
- orte->motion_status.err_right =
- (frame.data[2]<<8)|(frame.data[3]);
- if(++status_cnt == 5) {
- ORTEPublicationSend(orte->publicationMotionStatus);
- status_cnt = 0;
- }
- /*
- if(ms.err_left || ms.err_right)
- printf("MOTOR STATUS: left 0x%x, right 0x%x\n",
- ms.err_left, ms.err_right);
- */
- break;
-
- /* long sharps */
- case CAN_ADC_1:
- /* TODO: zkontrolovat, zda hodnoty jsou spravne v mm !! */
- orte->sharps_oponent.longSharpDist1 =
- s_ir2mmLong((frame.data[0]<<8)|(frame.data[1]))/1000.0;
- orte->sharps_oponent.longSharpDist2 =
- s_ir2mmLong((frame.data[2]<<8)|(frame.data[3]))/1000.0;
- orte->sharps_oponent.longSharpDist3 =
- s_ir2mmLong((frame.data[4]<<8)|(frame.data[5]))/1000.0;
- orte->front_door.state = (frame.data[6]<<8)|frame.data[7];
-
- if(++adc1_cnt == 5) {
- ORTEPublicationSend(orte->publicationSharp1);
- adc1_cnt = 0;
- }
- break;
-
- /* short sharps */
- case CAN_ADC_2:
- orte->sharps_waste.short1 = (frame.data[0]<<8)|(frame.data[1]);
- orte->sharps_waste.short2 = (frame.data[2]<<8)|(frame.data[3]);
- orte->sharps_waste.short3 = (frame.data[4]<<8)|(frame.data[5]);
- orte->sharps_waste.short4 = (frame.data[6]<<8)|(frame.data[7]);
-
- if(++adc2_cnt == 5) {
- ORTEPublicationSend(orte->publicationSharp2);
- adc2_cnt = 0;
- }
- break;
-
- /* sharps */
- case CAN_ADC_3:
- orte->adcs.sharpLong1 = (frame.data[0]);
- orte->adcs.sharpLong2 = (frame.data[1]);
- orte->adcs.sharpLong3 = (frame.data[2]);
- orte->adcs.sharpShort1 = (frame.data[3]);
- orte->adcs.sharpShort2 = (frame.data[4]);
- orte->adcs.sharpShort3 = (frame.data[5]);
- orte->adcs.sharpShort4 = (frame.data[6]);
- orte->adcs.frontDoor = (frame.data[7]);
-
- if(++adc3_cnt == 5) {
- ORTEPublicationSend(orte->publicationAdcs);
- adc3_cnt = 0;
- }
- break;
-
- /* IR inner sensors */
- case CAN_IR:
- orte->inner_ir.front = frame.data[1];
- orte->inner_ir.back = frame.data[0];
- ORTEPublicationSend(orte->publicationIR);
-
- orte->dig_in.state = frame.data[2];
- if(++ir_cnt == 5) {
- ORTEPublicationSend(orte->publicationDI);
- ir_cnt = 0;
- }
- break;
-
- /* laser data */
- case CAN_LAS1:
- printf("CAN: ");
- for (i=0; i<frame.can_dlc; i++) {
- printf("0x%02x ", frame.data[i]);
- }
- printf("can_dlc=%d\n", frame.can_dlc);
- orte->laser.cnt = frame.data[1];
- las_bcnt = orte->laser.cnt;
- last_id = frame.data[0];
- las_di = 4;
- /* rotation period */
- orte->laser.period = (frame.data[2]<<8)|(frame.data[3]);
- printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
- frame.can_id, orte->laser.cnt, orte->laser.period);
-
- for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
- switch (las_mi) {
- case 0: las_meas = &orte->laser.measures0; break;
- case 1: las_meas = &orte->laser.measures1; break;
- default: break;
- }
- *las_meas =
- (frame.data[las_di++]<<8)|(frame.data[las_di++]);
- printf("0x%02x 0x%02x %u ",
- frame.data[las_di-2], frame.data[las_di-1], *las_meas);
- }
- printf("\n");
- break;
- case CAN_LAS2:
- case CAN_LAS3:
- case CAN_LAS4:
- printf("CAN: ");
- for (i=0; i<frame.can_dlc; i++) {
- printf("0x%02x ", frame.data[i]);
- }
- printf("can_dlc=%d\n", frame.can_dlc);
- printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
- frame.can_id, orte->laser.cnt, orte->laser.period);
- if (frame.data[0] != (last_id+(frame.can_id-CAN_LAS1)))
- break;
- las_di = 2;
- while (las_di < 8 && las_bcnt > 0) {
- switch (las_mi) {
- case 2: las_meas = &orte->laser.measures2; break;
- case 3: las_meas = &orte->laser.measures3; break;
- case 4: las_meas = &orte->laser.measures4; break;
- case 5: las_meas = &orte->laser.measures5; break;
- case 6: las_meas = &orte->laser.measures6; break;
- case 7: las_meas = &orte->laser.measures7; break;
- case 8: las_meas = &orte->laser.measures8; break;
- case 9: las_meas = &orte->laser.measures9; break;
- default: break;
- }
- *las_meas =
- (frame.data[las_di++]<<8)|(frame.data[las_di++]);
- printf("0x%02x 0x%02x %u ",
- frame.data[las_di-2], frame.data[las_di-1], *las_meas);
- las_mi++;
- las_bcnt--;
- }
- printf("\n");
- if (las_bcnt == 0) {
- printf("ORTEPublicationSend()\n");
- ORTEPublicationSend(orte->publicationLaser);
- }
- break;
- default:
-// printf("received CAN msg with unknown id: %x\n",frame.can_id);
- break;
- }
-}
-
-void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct eb2007_orte_data *orte_data = (struct eb2007_orte_data *)recvCallBackParam;
-
- switch (info->status) {
- case NEW_DATA:
- set_serva(orte_data);
- break;
- case DEADLINE:
- printf("ORTE deadline occurred - servo receive\n");
- break;
- }
-}
-
-void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct eb2007_orte_data *orte_data = (struct eb2007_orte_data *)recvCallBackParam;
-
- switch (info->status) {
- case NEW_DATA:
- set_motor_speed(orte_data);
- /*printf("motor cmd received\n");*/
- break;
- case DEADLINE:
- printf("motor cmd deadline occurred, stopping motors\n");
- orte_data->motion_speed.left = 0;
- orte_data->motion_speed.right = 0;
- set_motor_speed(orte_data);
- break;
- }
-}
-
-int main(int argc, char *argv[])
-{
- fd_set read_fd_set;
- struct timeval timeout;
- int ret;
- int size;
- int rv = 0;
-
- struct eb2007_orte_data orte;
- struct can_frame frame;
-
- if (cand_init() < 0) {
- printf("cand: init failed\n");
- exit(1);
- } else {
- printf("cand: init OK\n");
- }
-
- orte.strength = 1;
-
- /* orte initialization */
- eb2007_roboorte_init(&orte);
-
- /* creating publishers */
- eb2007_publisher_motionstatus_create(&orte, NULL);
- eb2007_publisher_position_create(&orte, NULL);
- eb2007_publisher_fd_create(&orte, NULL);
- eb2007_publisher_ir_create(&orte, NULL);
- eb2007_publisher_sharpsouper_create(&orte, NULL);
- eb2007_publisher_sharplahve_create(&orte, NULL);
- eb2007_publisher_di_create(&orte, NULL);
- eb2007_publisher_adecka_create(&orte, NULL);
- eb2007_publisher_laser_create(&orte, NULL);
-
- /* creating subscribers */
- eb2007_subscriber_motor_create(&orte, rcv_motion_speed_cb);
- eb2007_subscriber_serva_create(&orte, rcv_servos_cb);
-
- /* main loop */
- for(;;) {
- FD_ZERO(&read_fd_set);
- FD_SET(sock, &read_fd_set);
- timeout.tv_sec = 0;
- timeout.tv_usec = 10000;
-
- ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
- if (ret < 0) {
- perror("cand: select() error");
- goto err;
- }
-
- /* timeout expired, nothing to be done */
- if (ret == 0)
- continue;
-
- if (!FD_ISSET(sock, &read_fd_set))
- continue;
-
- /* read data from SocketCAN */
- size = read(sock, &frame, sizeof(struct can_frame));
-
- /* parse data */
- cand_parse_frame(&orte, frame);
- }
-
-err:
- close(sock);
- return 1;
-out:
- return EXIT_SUCCESS;
-}
+++ /dev/null
-/*
- * cand_eb2007.h 08/04/07
- *
- * CAN daemon for the Eurobot 2007 competition.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#ifndef CAND_EB2007_H
-#define CAND_EB2007_H
-
-int sock;
-struct sockaddr_can can_addr;
-struct can_filter rfilter;
-struct ifreq can_ifreq;
-uint8_t last_id;
-
-/* unused */
-/* short int mL, mR;
-uint16_t las_data[12];
-uint16_t stateIR;
-uint8_t stateDI;
-uint16_t stateSharp1[4];
-uint16_t stateSharp2[4];
-uint8_t serva[8];*/
-
-/* function prototypes */
-/*short int convertShortInt(short int x);*/
-int can_send(canid_t id, unsigned char length, unsigned char *data);
-int set_motor_speed();
-int set_serva();
-
-#endif /* CAND_EB2007_H */
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-
-bin_PROGRAMS += cand_eb2008
-cand_eb2008_SOURCES = cand_eb2008.cc
-cand_eb2008_LIBS = pthread sharp roboorte_eb2008 roboorte_generic robottype robottype_eb2008 orte
#include <can_msg_masks.h>
#include <can_msg_def.h>
#include <can_msg_masks.h>
-#include "cand_eb2008.h"
+#include "cand.h"
#define CAN_SONAR 0x60
int cand_init()
{
include_HEADERS = hokuyo.h
-lib_LOADLIBES = pthread roboorte_generic robottype orte
+lib_LOADLIBES = pthread roboorte robottype orte
#include <string.h>
#include "hokuyo.h"
#include <robottype.h>
-#include <roboorte_generic.h>
+#include <roboorte_robottype.h>
static FILE *dev;
u_int16_t *scan;
-struct generic_orte_data gorte;
+struct robottype_orte_data orte;
// SERIAL PORT RELATED FUNCTIONS
return -1;
}
- generic_roboorte_init(&gorte);
- generic_publisher_hokuyo_scan_create(&gorte, scan_publisher_callback, NULL);
+ robottype_roboorte_init(&orte);
+ robottype_publisher_hokuyo_scan_create(&orte, scan_publisher_callback, NULL);
return 0;
joyd_SOURCES = joyd.cc
-joyd_LIBS = stdc++ pthread roboorte_generic robottype robottype_eb2008 orte
+joyd_LIBS = stdc++ pthread roboorte robottype orte
#include <time.h>
#include <math.h>
#include <signal.h>
-#include <roboorte_generic.h>
+#include <roboorte_robottype.h>
#define JOY_DEV "/dev/input/js0"
#define VMAX 16000
-struct generic_orte_data gorte;
+struct robottype_orte_data orte;
int joy_fd;
static void int_handler(int sig)
printf("joyd stopping.\n");
/* orte domain destroy */
- generic_roboorte_destroy(&gorte);
+ robottype_roboorte_destroy(&orte);
/* close file descriptor */
close(joy_fd);
signal(SIGINT, int_handler);
- gorte.strength = 30;
+ orte.strength = 30;
/* orte initialization */
- if (generic_roboorte_init(&gorte)) {
+ if (robottype_roboorte_init(&orte)) {
printf("Unable to initialize ORTE.\n");
exit(1);
}
/* creating publishers */
- generic_publisher_motion_speed_create(&gorte, NULL, &gorte);
- generic_publisher_joy_data_create(&gorte, NULL, &gorte);
+ robottype_publisher_motion_speed_create(&orte, NULL, &orte);
+ robottype_publisher_joy_data_create(&orte, NULL, &orte);
/* open joystick device */
if((joy_fd=open(JOY_DEV, O_NONBLOCK))==-1) {
break;
}
- gorte.joy_data.axisX = axis[0];
- gorte.joy_data.axisY = axis[1];
- gorte.joy_data.axisZ = axis[2];
- gorte.joy_data.axisR = axis[3];
- gorte.joy_data.axisS1 = axis[4];
- gorte.joy_data.axisS2 = axis[5];
-
- gorte.joy_data.button1 = button[0];
- gorte.joy_data.button2 = button[1];
- gorte.joy_data.button3 = button[2];
- gorte.joy_data.button4 = button[3];
- gorte.joy_data.button5 = button[4];
- gorte.joy_data.button6 = button[5];
- gorte.joy_data.button7 = button[6];
- gorte.joy_data.button8 = button[7];
- gorte.joy_data.button9 = button[8];
- gorte.joy_data.button10 = button[9];
- gorte.joy_data.button11 = button[10];
- gorte.joy_data.button12 = button[11];
+ orte.joy_data.axisX = axis[0];
+ orte.joy_data.axisY = axis[1];
+ orte.joy_data.axisZ = axis[2];
+ orte.joy_data.axisR = axis[3];
+ orte.joy_data.axisS1 = axis[4];
+ orte.joy_data.axisS2 = axis[5];
+
+ orte.joy_data.button1 = button[0];
+ orte.joy_data.button2 = button[1];
+ orte.joy_data.button3 = button[2];
+ orte.joy_data.button4 = button[3];
+ orte.joy_data.button5 = button[4];
+ orte.joy_data.button6 = button[5];
+ orte.joy_data.button7 = button[6];
+ orte.joy_data.button8 = button[7];
+ orte.joy_data.button9 = button[8];
+ orte.joy_data.button10 = button[9];
+ orte.joy_data.button11 = button[10];
+ orte.joy_data.button12 = button[11];
/* v = (double)(axis[0]/32768.0);// * 10000;
omega = (double)(axis[1]/32768.0);// * 10000;
- gorte.motion_speed.right = -(v + r*omega);
- gorte.motion_speed.left = v - r*omega;*/
+ orte.motion_speed.right = -(v + r*omega);
+ orte.motion_speed.left = v - r*omega;*/
v = (double)axis[1]/32768.0;
omega = (double)axis[0]/32768.0;
lenght = sqrt( pow(v,2) + pow(omega,2));
omega = cos(angle);
}
omega *= 2;
- gorte.motion_speed.right = (int)(-((v + r*omega))*VMAX);
- gorte.motion_speed.left = (int)(-(v - r*omega)*VMAX);
- printf("gorte.motion_speed.left=%d, gorte.motion_speed.right=%d\n",
- gorte.motion_speed.left,gorte.motion_speed.right);
+ orte.motion_speed.right = (int)(-((v + r*omega))*VMAX);
+ orte.motion_speed.left = (int)(-(v - r*omega)*VMAX);
+ printf("orte.motion_speed.left=%d, orte.motion_speed.right=%d\n",
+ orte.motion_speed.left,orte.motion_speed.right);
printf("v %f, omega %f\n",v, omega);//#endif
printf("length %f, angle %f\n",lenght, angle);//#endif
printf("axisX %d, axisY %d\n",axis[0], axis[1]);//#endif
- ORTEPublicationSend(gorte.publication_motion_speed);
- ORTEPublicationSend(gorte.publication_joy_data);
+ ORTEPublicationSend(orte.publication_motion_speed);
+ ORTEPublicationSend(orte.publication_joy_data);
}
- ORTEPublicationSend(gorte.publication_motion_speed);
- ORTEPublicationSend(gorte.publication_joy_data);
+ ORTEPublicationSend(orte.publication_motion_speed);
+ ORTEPublicationSend(orte.publication_joy_data);
nanosleep(&timer,NULL);
/* printf( "X: %6d Y: %6d ", axis[0], axis[1] ); */
#include <math.h>
#include <robomath.h>
#include <string.h>
-#include <robodim_eb2008.h>
+#include <robodim.h>
#include "mcl.h"
#ifdef MATLAB_MEX_FILE
#include <mcl.h>
#include <mcl_robot.h>
-#include <robodim_eb2008.h>
+#include <robodim.h>
#include <stdint.h>
#define mcl_to_laser(mcl) (struct mcl_laser*)((mcl)->priv)
test_PROGRAMS += testmcl_performance
testmcl_performance_SOURCES = testmcl_performance.c
-testmcl_performance_LIBS = mcl shist robomath m rt robodim_eb2008
+testmcl_performance_LIBS = mcl shist robomath m rt robodim
#include <mcl_laser.h>
#include <math.h>
#include <robomath.h>
-#include <robodim_eb2007.h>
+#include <robodim.h>
#include <shist.h>
#define TEST_COUNT 500
# -*- makefile -*-
# Robot's ORTE library
-SUBDIRS = eb2007 eb2008
-
+include_HEADERS = robodim.h
+lib_LIBRARIES = robodim
+robodim_SOURCES = robodim.c
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-
-include_HEADERS = robodim_eb2007.h
+++ /dev/null
-/*
- * robodim_eb2007.h 07/08/01
- *
- * Dimensions for the robot, playground and other stuffs.
- * Eurobot 2007
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#ifndef ROBODIM_EB2007_H
-#define ROBODIM_EB2007_H
-
-/**
- * ROBOT DIMENSIONS
- *
- * +--------------------------+
- * ^ | | | |---|
- * ^ | | ------- ABE | |
- * RR| | | :<--------------------->|
- * v | | : | |
- * W| | + Center (L) | |
- * | | AB : AF | |
- * | |<---->:<----------------->| |
- * | | ------- | |
- * v | | | |---|
- * +--------------------------+
- * <-->
- * WR
- */
-
-#define ROBOT_WIDTH_MM 290 /* W*/
-#define ROBOT_WIDTH_M (ROBOT_WIDTH_MM/1000.0)
-#define ROBOT_ROTATION_RADIUS_MM (254/2) /* R*/
-#define ROBOT_ROTATION_RADIUS_M (ROBOT_ROTATION_RADIUS_MM/1000.0)
-#define LASER_CENTER_OFFSET_MM 50 /*FIXME*/ /* Distance of the laser from rotation axis (Center to L) */
-#define LASER_CENTER_OFFSET_M (LASER_CENTER_OFFSET_MM/1000.0)
-#define ROBOT_WHEEL_RADIUS_MM 40 /* WR */
-#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM/1000.0)
-#define ROBOT_AXIS_TO_BACK_MM 75 /* AB */
-#define ROBOT_AXIS_TO_BACK_M (ROBOT_AXIS_TO_BACK_MM/1000.0)
-#define ROBOT_AXIS_TO_FRONT_MM 183 /* AF */
-#define ROBOT_AXIS_TO_FRONT_M (ROBOT_AXIS_TO_FRONT_MM/1000.0)
-#define ROBOT_AXIS_TO_BELT_MM 280 /* ABE */
-#define ROBOT_AXIS_TO_BELT_M (ROBOT_AXIS_TO_BELT_MM/1000.0)
-
-
-/**
- * PLAYGROUND DIMENSIONS
- *
- * D=S2 F C=R3
- * +-------------+-------------+
- * ^ | |
- * | | |
- * | | +H |
- * | | |
- * | | |
- * H | R1+ +L=S1
- * | =K| |
- * | | +r |
- * | | +G |
- * | | |
- * v | |
- * +-------------+-------------+
- * A=S3 E B=R2
- * <------------------------->
- * W
- *
- * Fixed points (known points) [x,y]
- * A - [0,0]
- * B - [width,0]
- * C - [width,height]
- * D - [0,height]
- * E - [width/2,0]
- * F - [width/2,height]
- * G - [width/2,height/4]
- * H - [width/2,height/4*3]
- * K - [0,height/2]
- * L - [width,height/2]
- * Our reflectors
- * R1, R2, R3
- * Opponent`s reflectors
- * S1, S2, S3
- *
- * Unknown point
- * r - Position of the robot
- *
- * Angles:
- * theta1 = from robot`s head to the first reflector
- * theta2 = from robot`s head to the second reflector
- * theta3 = from robot`s head to the third reflector
- * alfa = angle R3-r-R2 (S2-r-S3)
- * beta = angle R1-r-R3 (S3-r-S1)
- * gama = angle R1-r-R2 (S1-r-S2)
- *
- * Angle`s sense - counterclockwise
- *
- */
-#define PLAYGROUND_WIDTH_MM 3000
-#define PLAYGROUND_WIDTH_M (PLAYGROUND_WIDTH_MM/1000.0)
-#define PLAYGROUND_HEIGHT_MM 2100
-#define PLAYGROUND_HEIGHT_M (PLAYGROUND_HEIGHT_MM/1000.0)
-
-#endif /* ROBODIM_EB2007_H */
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-
-include_HEADERS = robodim_eb2008.h
-lib_LIBRARIES = robodim_eb2008
-robodim_eb2008_SOURCES = robodim_eb2008.c
-#include "robodim_eb2008.h"
+#include "robodim.h"
#include <robomath.h>
/* Beacon positions. For some version of MCL, this must be sorted in
# -*- makefile -*-
-SUBDIRS = eb2008 utils
+SUBDIRS = utils test
+
+default_CONFIG = CONFIG_OPEN_LOOP=n CONFIG_LOCK_CHECKING=n
+
+config_include_HEADERS = robot_config.h
+robot_config_DEFINES = CONFIG_OPEN_LOOP CONFIG_LOCK_CHECKING
+
+bin_PROGRAMS += robomain
+robomain_SOURCES = main.cc fsmmain.c
+
+# Library with general support functions for the robot
+lib_LIBRARIES += robot
+robot_SOURCES = robot_orte.c servos.c robot.c fsmmove.cc fsmjoy.c \
+ movehelper.cc fsmdisplay.c fsmloc.c map_handling.c
+robot_GEN_SOURCES = roboevent.c
+include_GEN_HEADERS += roboevent.h
+
+include_HEADERS += robot.h servos.h movehelper.h robot_orte.h
+
+# Libraries linked to all programs in this Makefile
+lib_LOADLIBES = robot mcl robomath roboorte robottype robottype \
+ pthread rt m orte pathplan sharp map fsm uoled oledlib \
+ rbtree motion robodim sercom
+
+# Automatic generation of event definition files
+include-pass_HOOKS = roboevent.c roboevent.h
+
+roboevent.c roboevent.h: $(SOURCES_DIR)/roboevent.py $(SOURCES_DIR)/../fsm/eventgen.py
+ @$(QUIET_CMD_ECHO) " EVENTGEN roboevent"
+ $(Q)python $(SOURCES_DIR)/../fsm/eventgen.py $<
+
+clean-custom:
+ $(Q)rm -f roboevent.c roboevent.h
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-
-SUBDIRS = test
-
-default_CONFIG = CONFIG_OPEN_LOOP=n
-LOCAL_CONFIG_H = robot_config.h
-
-bin_PROGRAMS += robomain
-robomain_SOURCES = fsmmain.cc
-
-# Library with general support functions for the robot
-lib_LIBRARIES += robot_eb2007
-robot_eb2007_SOURCES = servos_eb2007.c robot_eb2007.c roboorte.c \
- fsmmove.cc fsmpickup.c fsmdet.c fsmjoy.c fsmloc.c \
- movehelper_eb2007.cc
-robot_eb2007_GEN_SOURCES = roboevent_eb2007.c
-include_GEN_HEADERS += roboevent_eb2007.h
-
-include_HEADERS += robot_eb2007.h servos_eb2007.h movehelper_eb2007.h
-
-# Libraries linked to all programs in this Makefile
-lib_LOADLIBES = robot_eb2007 mcl laser-nav robomath robottype robottype_eb2007 pthread \
- rt m orte pathplan sharp map fsm rbtree motion
-
-# Automatic generation of event definition files
-include-pass_HOOKS = roboevent_eb2007.c roboevent_eb2007.h
-
-roboevent_eb2007.c roboevent_eb2007.h: $(SOURCES_DIR)/roboevent_eb2007.py $(SOURCES_DIR)/../../fsm/eventgen.py
- @$(QUIET_CMD_ECHO) " EVENTGEN roboevent"
- $(Q)python $(SOURCES_DIR)/../../fsm/eventgen.py $<
-
-clean-custom:
- $(Q)rm -f roboevent_eb2007.c roboevent_eb2007.h
+++ /dev/null
-/*
- * detfsm.c 07/04/20
- *
- * Finite state machine of the detection part.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_DETECT
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <fsm.h>
-#include <movehelper_eb2007.h>
-#include <servos_eb2007.h>
-#include <math.h>
-
-#define WASTE_STACK_EMPTYm 0xe0
-#define WASTE_INNER_STACK_BOTTLE1 0x1e
-#define WASTE_INNER_STACK_BOTTLE2 0x0f
-
-#define CLOSE_LIMIT 850
-#define FAR_LIMIT 650
-
-#define VMAX 0.5;
-//#define JOYSTICK_NAV 1
-#define R 0.15
-
-void evaluate_distance(uint16_t value, uint8_t *result) {
- if((value > CLOSE_LIMIT)) *result = CLOSE;
- else if((value >= FAR_LIMIT)&&(value < CLOSE_LIMIT)) *result = FAR;
- else if(value < FAR_LIMIT) *result = NOTHING;
-}
-
-FSM_STATE_DECL(det_start);
-FSM_STATE_DECL(det_scan);
-
-FSM_STATE(det_start) {
- FSM_TIMER(100);
-// if(robot.start) {
- FSM_SIGNAL(MAIN, EV_START, NULL);
- FSM_TRANSITION(det_scan);
-// }
-}
-
-FSM_STATE(det_scan) {
-
-// ROBOT_LOCK();
-
- robot.waste_cnt = 0;
-/* if(robot.sharpsWaste.short1 > VERY_FAR_LIMIT) robot.waste_cnt++;
- if(robot.sharpsWaste.short2 > VERY_FAR_LIMIT) robot.waste_cnt++;
- if(robot.sharpsWaste.short3 > VERY_FAR_LIMIT) robot.waste_cnt++;
- if(robot.sharpsWaste.short4 > VERY_FAR_LIMIT) robot.waste_cnt++;*/
-
-// printf("waste_cnt=%d\n",robot.waste_cnt);
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(100);
- break;
- case EV_TIMER:
- FSM_TIMER(100);
- //printf("EVENT_STATE_ENTERED - pickup ready\n");
- if(robot.waste_cnt) {
- evaluate_distance(robot.sharpsWaste.short1, &robot.bott_sens.sens1);
- evaluate_distance(robot.sharpsWaste.short2, &robot.bott_sens.sens2);
- evaluate_distance(robot.sharpsWaste.short3, &robot.bott_sens.sens3);
- evaluate_distance(robot.sharpsWaste.short4, &robot.bott_sens.sens4);
-
- FSM_SIGNAL(MAIN, EV_DETECTED_BOTTLE, NULL);
- break;
- }
-
- // parsing joy buttons
-
- if(robot.joy.button1 == 1) innerTransporterStop();
- if(robot.joy.button2 == 1) frontTransporterStop();
- if(robot.joy.button3 == 1) frontDoorDown();
- if(robot.joy.button4 == 1) frontTransporterBackward();
- if(robot.joy.button5 == 1) frontDoorUp();
- if(robot.joy.button6 == 1) frontTransporterForward();
- if(robot.joy.button7 == 1) innerDoorDown();
- if(robot.joy.button8 == 1) innerDoorUp();
- if(robot.joy.button9 == 1) backDoorDown();
- if(robot.joy.button10 == 1) backDoorUp();
- if(robot.joy.button11 == 1) innerTransporterLeft();
- if(robot.joy.button12 == 1) innerTransporterRight();
-
-#ifdef JOYSTICK_NAV // if joystick is used for navigating
- v = (double)robot.joy.axisY/32768;
- omega = (double)robot.joy.axisX/32768;
- lenght = sqrt( pow(v,2) + pow(omega,2));
- if (lenght >= 1) // If lenght si more than 1 is used unit circle
- {
- angle = tan2(robot.joy.axisX/robot.joy.axisY); // computing new speed and omega
- v = sin(angle);
- omega = cos(angle);
- }
-
- speedL = (v - R*omega)*VMAX;
- speedR = -(v + R*omega)*VMAX;
- robot_send_speed(speedL, speedR); // forcing speed command
- printf("v %f, omega %f\n",v, omega);//#endif
- printf("length %f, angle %f\n",lenght, angle);//#endif
- printf("axisX %d, axisY %d\n",robot.joy.axisX, robot.joy.axisY);//#endif
- printf("sent speed: %f, %f\n",speedL, speedR);//#endif
-#endif
- break;
- default:
- DBG_PRINT_EVENT("Unhandled event:");
- }
-
-// ROBOT_UNLOCK();
-}
-
-/*
- // definovat nove ev na joy
- EV_JOY_B0
- EV_JOY_B1
- EV_JOY_B2
- EV_JOY_B0
- EV_JOY_B4
- EV_JOY_B5
- EV_JOY_B6
- EV_JOY_B7
- EV_JOY_B8
- EV_JOY_B9
- EV_JOY_B10
- EV_JOY_B11
-
-
-
-*/
+++ /dev/null
-/*
- * fsmjoy.c 07/10/11
- *
- * Finite state machine of the joystick control.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_JOYSTICK
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <fsm.h>
-#include <movehelper_eb2007.h>
-#include <servos_eb2007.h>
-#include <math.h>
-
-#define CLOSE_LIMIT 850
-#define FAR_LIMIT 650
-
-#define VMAX 0.5;
-#define R 0.15
-
-//#define JOYSTICK_NAV
-
-FSM_STATE_DECL(joy_init);
-FSM_STATE_DECL(joy_scan);
-
-FSM_STATE(joy_init) {
- printf("joystick control initialization\n");
- FSM_TRANSITION(joy_scan);
-}
-
-FSM_STATE(joy_scan) {
-
- FSM_TIMER(100);
- switch (FSM_EVENT) {
- case EV_ENTRY:
- break;
- case EV_TIMER:
- if(robot.joy.button1 == 1) innerTransporterStop();
- if(robot.joy.button2 == 1) frontTransporterStop();
- if(robot.joy.button3 == 1) frontDoorDown();
- if(robot.joy.button4 == 1) frontTransporterBackward();
- if(robot.joy.button5 == 1) frontDoorUp();
- if(robot.joy.button6 == 1) frontTransporterForward();
- if(robot.joy.button7 == 1) innerDoorDown();
- if(robot.joy.button8 == 1) innerDoorUp();
- if(robot.joy.button9 == 1) backDoorDown();
- if(robot.joy.button10 == 1) backDoorUp();
- if(robot.joy.button11 == 1) innerTransporterLeft();
- if(robot.joy.button12 == 1) innerTransporterRight();
-
-#ifdef JOYSTICK_NAV // if joystick is used for navigating
- v = (double)robot.joy.axisY/32768;
- omega = (double)robot.joy.axisX/32768;
- lenght = sqrt( pow(v,2) + pow(omega,2));
- if (lenght >= 1) // If lenght si more than 1 is used unit circle
- {
- angle = tan2(robot.joy.axisX/robot.joy.axisY); // computing new speed and omega
- v = sin(angle);
- omega = cos(angle);
- }
-
- speedL = (v - R*omega)*VMAX;
- speedR = -(v + R*omega)*VMAX;
- robot_send_speed(speedL, speedR); // forcing speed command
- printf("v %f, omega %f\n",v, omega);//#endif
- printf("length %f, angle %f\n",lenght, angle);//#endif
- printf("axisX %d, axisY %d\n",robot.joy.axisX, robot.joy.axisY);//#endif
- printf("sent speed: %f, %f\n",speedL, speedR);//#endif
-#endif
- break;
- default:
- DBG_PRINT_EVENT("Unhandled event:");
- }
-}
-
-/*
- // definovat nove ev na joy
- EV_JOY_B0
- EV_JOY_B1
- EV_JOY_B2
- EV_JOY_B0
- EV_JOY_B4
- EV_JOY_B5
- EV_JOY_B6
- EV_JOY_B7
- EV_JOY_B8
- EV_JOY_B9
- EV_JOY_B10
- EV_JOY_B11
-*/
+++ /dev/null
-/*
- * @(#)locfsm.c 07/04/19
- *
- * Finite state machine of the localization part.
- *
- * Copyright : (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * Contacts : Tran Duy Khanh <trandk1@fel.cvut.cz>
- * License : GNU GPL v.2
- */
-
-#define FSM_LOC
-#include <math.h>
-#include <robomath.h>
-#include <laser-nav.h>
-#include <mcl.h>
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <fsm.h>
-
-/* FIXME: not used, need to updated with the new mcl api */
-
-// struct mcl_particle cal_pos;
-// void loc_pln_update();
-// int loc_mcl_update(struct mcl_model *mcl, double dx, double dy, double dangle);
-// void dbg_print_parts();
-
-FSM_STATE_DECL(loc_init);
-// FSM_STATE_DECL(loc_run);
-// FSM_STATE_DECL(loc_mcl_init);
-// FSM_STATE_DECL(loc_update);
-
-/**
- * FSM state: firste state. Initilization.
- */
-FSM_STATE(loc_init)
-{
- /* set reference points. Used to calculate position of the robot
- * using laser */
-// pln_set_points();
-// FSM_TIMER(1000);
-// FSM_TRANSITION(loc_mcl_init);
-}
-
-// /**
-// * FSM state: MCL initialization.
-// */
-// FSM_STATE(loc_mcl_init)
-// {
-// struct mcl_model *mcl = (struct mcl_model *)&robot.mcl;
-//
-// /* playground size */
-// mcl->width = PLAYGROUND_WIDTH_MM;
-// mcl->height = PLAYGROUND_HEIGHT_MM;
-//
-// /* the noises */
-// mcl->gen_dnoise = 0.99; /* distance noise */
-// mcl->gen_anoise = DEGREES(10); /* angle noise */
-// mcl->mov_dnoise = 1.1;
-// mcl->mov_anoise = 2.0;
-// mcl->w_min = 0.25;
-// mcl->w_max = 2.0;
-// mcl->eval_sigma = 0.5;
-// mcl->maxavdist = 50;
-// mcl->maxnoisecycle = 10; /* bad cycles before reseting */
-//
-// /* amount of particles */
-// mcl->count = 5000;
-// mcl->parts = (struct mcl_particle *)
-// malloc(sizeof(struct mcl_particle)*mcl->count);
-//
-// /* measured data */
-// mcl->data =
-//
-// /* phases of the MCL */
-// mcl->init = mcl_init;
-// mcl->move = mcl_move;
-// mcl->update = mcl_update;
-// mcl->normalize = mcl_normalize;
-// mcl->sort = mcl_partsort;
-// mcl->resample = mcl_resample;
-//
-// /* cycles of enumeration */
-// mcl->cycle = 0;
-// mcl->noisecycle = 0;
-//
-// /* generate particles with noises */
-// mcl->init(mcl);
-// // printf("loc_mcl_init: done\n");
-//
-// /* change flag */
-// mcl->flag = MCL_RUN;
-//
-// /* print generated particles */
-// /*dbg_print_parts();*/
-//
-// FSM_TRANSITION(loc_run);
-// }
-//
-// /**
-// * FSM state: main state. Every state will return back to this one.
-// */
-// FSM_STATE(loc_run)
-// {
-// // DBG_PRINT_EVENT("loc_run event:");
-//
-// // FSM_TIMER(1000);
-// switch (FSM_EVENT) {
-// case EV_LOC_INIT:
-// /*DBG_PRINT_EVENT("mcl_init event:");*/
-// FSM_TRANSITION(loc_mcl_init);
-// break;
-// // case EV_TIMEOUT:
-// // DBG_PRINT_EVENT("timeout event:");
-// // break;
-// case EV_LOC_LOCATE:
-// case EV_LASER_RECEIVED:
-// SUBFSM_TRANSITION(loc_update);
-// break;
-// default:
-// /*DBG_PRINT_EVENT("loc_run:unhandled event:");*/
-// ;
-// }
-// }
-//
-// /**
-// * FSM state: update estimated position. Calculate position from measured
-// * times, then continue updating the mcl.
-// */
-// FSM_STATE(loc_update)
-// {
-// int dx, dy, dangle;
-// int rv;
-//
-// // DBG_PRINT_EVENT("loc_update:");
-//
-// /* FIXME: here should be data from the odometry */
-// ROBOT_LOCK(act_pos);
-// dx = 1.0;
-// dy = 1.0;
-// dangle = 0.0;
-// ROBOT_UNLOCK(act_pos);
-//
-// /* calculate measured position */
-// loc_pln_update();
-//
-// /* update mcl with measured position */
-// // rv = loc_mcl_update(&robot.mcl, (double)dx, (double)dy, (double)dangle);
-//
-// // /* send update signal if mcl update has finished successful */
-// // if (!rv)
-// // FSM_SIGNAL(MOTION, EV_LOC_UPDATED);
-// // else {
-// // printf("MCL_RESET: LOST!\n");
-// // FSM_SIGNAL(MOTION, EV_LOC_LOST);
-// // }
-//
-// SUBFSM_RET();
-// }
-//
-// /**
-// * Calculated position from measured times.
-// */
-// void loc_pln_update()
-// {
-// struct pln_pos_state _act_pos;
-// struct pln_pos_state _cal_pos;
-// unsigned int times[LAS_CNT], cnt;
-// int i;
-//
-// /* measured times from laser beacon */
-// ROBOT_LOCK(laser_recv);
-// /* FIXME: just a hack, until the laser will
-// * work correctly */
-// // robot.laser_recv.cnt = 5;
-// // robot.laser_recv.period = 54619;
-// // robot.laser_recv.measures0 = 1714;
-// // robot.laser_recv.measures1 = 12312;
-// // robot.laser_recv.measures2 = 34549;
-// // robot.laser_recv.measures3 = 41231;
-// // robot.laser_recv.measures4 = 49670;
-//
-// cnt = robot.laser_recv.cnt + 1;
-// times[0] = robot.laser_recv.period;
-// times[1] = robot.laser_recv.measures0;
-// times[2] = robot.laser_recv.measures1;
-// times[3] = robot.laser_recv.measures2;
-// times[4] = robot.laser_recv.measures3;
-// times[5] = robot.laser_recv.measures4;
-// times[6] = robot.laser_recv.measures5;
-// times[7] = robot.laser_recv.measures6;
-// times[8] = robot.laser_recv.measures7;
-// times[9] = robot.laser_recv.measures8;
-// times[10] = robot.laser_recv.measures9;
-// ROBOT_UNLOCK(laser_recv);
-//
-// printf("\n");
-// printf("cnt=%d period=%d measures: ",
-// robot.laser_recv.cnt, robot.laser_recv.period);
-// printf("times:");
-// for (i=1; i<cnt; i++)
-// printf("%d ", times[i]);
-// printf("\n");
-//
-// /* actual position */
-// ROBOT_LOCK(act_pos);
-// _act_pos.x = robot.act_pos.x * 1000;
-// _act_pos.y = robot.act_pos.y * 1000;
-// _act_pos.head = robot.act_pos.phi;
-// ROBOT_UNLOCK(act_pos);
-// /* FIXME: this stays here just until the odometry will be done */
-// // _act_pos.x = 2000;
-// // _act_pos.y = 1950;
-// // _act_pos.head = DEG2RAD(0);
-// // printf("act: x=%f y=%f h=%f\n", _act_pos.x, _act_pos.y, _act_pos.head);
-//
-// /* calculated measured position */
-// pln_cal_position(times, cnt, _act_pos, &_cal_pos);
-// printf("calculated posititon: [x=%f,y=%f]\n", _cal_pos.x, _cal_pos.y);
-//
-// /* set currently calculated position */
-// /* FIXME: remove this when the mcl is used */
-// ROBOT_LOCK(est_pos);
-// robot.est_pos.x = _cal_pos.x / 1000;
-// robot.est_pos.y = _cal_pos.y / 1000;
-// robot.est_pos.phi = _cal_pos.head;
-// ROBOT_UNLOCK(est_pos);
-//
-// cal_pos.x = _cal_pos.x;
-// cal_pos.y = _cal_pos.y;
-// cal_pos.angle = _cal_pos.head;
-// }
-//
-// /**
-// * Move the particles and the robot by a specified distance.
-// *
-// * @param mcl the MCL model
-// * @param dx movement in axis X
-// * @param dy movement in axis Y
-// * @param dangle turning
-// */
-// int loc_mcl_update(struct mcl_model *mcl, double dx, double dy, double dangle)
-// {
-// struct mcl_particle est_pos;
-//
-// mcl->cycle++;
-// // printf("loc_mcl_update: cycle = %d\n", mcl->cycle);
-//
-// /* move particles */
-// mcl->move(mcl, dx, dy, dangle);
-//
-// /* update particles with calculated position from measured times */
-// mcl->update(mcl, cal_pos.x, cal_pos.y, cal_pos.angle);
-//
-// /* we got lost, reset mcl */
-// if (mcl->flag == MCL_RESET)
-// return 1;
-//
-// mcl->normalize(mcl);
-// mcl->sort(mcl);
-// mcl->resample(mcl);
-// est_pos = mcl_get_pos(mcl);
-//
-// /* estimated position in meter */
-// ROBOT_LOCK(est_pos);
-// robot.est_pos.x = est_pos.x / 1000;
-// robot.est_pos.y = est_pos.y / 1000;
-// robot.est_pos.phi = est_pos.angle;
-// ROBOT_UNLOCK(est_pos);
-//
-// // printf("estimated position: %f %f %f %f\n",
-// // est_pos.x, est_pos.y, est_pos.angle, est_pos.weight);
-//
-// return 0;
-// }
-//
-// /* Debug prints */
-// void dbg_print_parts()
-// {
-// int i;
-//
-// for (i=0; i<robot.mcl.count; i++)
-// printf("%d) x=%f y=%f a=%f w=%f\n",
-// i,
-// ((struct mcl_particle *)(robot.mcl.parts))[i].x,
-// ((struct mcl_particle *)(robot.mcl.parts))[i].y,
-// ((struct mcl_particle *)(robot.mcl.parts))[i].angle,
-// ((struct mcl_particle *)(robot.mcl.parts))[i].weight);
-// printf("robot: x=%f y=%f angle=%f\n",
-// ((struct mcl_particle *)(robot.mcl.system))->x,
-// ((struct mcl_particle *)(robot.mcl.system))->y,
-// ((struct mcl_particle *)(robot.mcl.system))->angle);
-//
-// }
+++ /dev/null
-/*
- * fsmmain.cc 07/10/01
- *
- * Robot's main control program (tested in Saint Peterburg).
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-/**
- * TODO:
- * test enemy ahead (sharp distance)
- * test enemy avoidance
- */
-#define FSM_MAIN
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <fsm.h>
-#include <unistd.h>
-#include <servos_eb2007.h>
-#include <math.h>
-#include "trgen.h"
-#include <movehelper_eb2007.h>
-#include <map.h>
-#include <sharp.h>
-#include <robomath.h>
-
-/* TODO: define this macro for the competition */
-#undef COMPETITION
-#ifdef COMPETITION
-#define WAIT_FOR_START
-#define STACK_SIZE_DEFAULT 1
-#define COMPETITION_TIME_DEFAULT 90
-#define WAIT_FOR_DEPOSITION_DEFAULT 60
-#define USE_WALL_TO_ALIGN
-#else
-#undef WAIT_FOR_START
-#define STACK_SIZE_DEFAULT 3
-#define COMPETITION_TIME_DEFAULT 900
-#define WAIT_FOR_DEPOSITION_DEFAULT 60
-#define USE_WALL_TO_ALIGN
-#endif
-
-/* competition time in seconds */
-#define COMPETITION_TIME COMPETITION_TIME_DEFAULT
-/* competition time in seconds */
-#define WAIT_FOR_DEPOSITION WAIT_FOR_DEPOSITION_DEFAULT
-/* maximal number of bottles before the deposition */
-#define STACK_SIZE STACK_SIZE_DEFAULT
-
-/**
- * Time constants.
- */
-/* time before stopping the deposition belt resp. closing the back door */
-#define DEPOSITE_WAIT 1000
-/* the same as above, but we need to wait a little bit longer for the last one*/
-#define LAST_DEPOSITE_WAIT 1500
-/* time before closing the inner door */
-#define WASTE_ROLL_OUT_WAIT 1000
-/* time before another pickup */
-#define NEW_PICK_UP_WAIT 1000
-/* waste detection period */
-#define WASTE_DETECTION_PERIOD 100
-
-#define GOTO_BASKET_MAX_ATTEMPTS 5
-#define ENEMY_AVOIDANCE_MAX_ATTEMPTS 5
-
-struct act_pos_type des_pos;
-int goto_basket_attempt_cnt = 0;
-int enemy_avoidance_attempt_cnt = 0;
-int no_align = 0;
-
-enum {
- LEFT = 0,
- CENTER,
- RIGHT
-};
-
-/**
- * Convert sharp`s measured values to mm.
- *
- * @ingroup fsmmain
- */
-void get_short_sharp_mm(int *sharp)
-{
- ROBOT_LOCK(sharpsWaste);
- sharp[0] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short1);
- sharp[1] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short2);
- sharp[2] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short3);
- sharp[3] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short4);
- ROBOT_UNLOCK(sharpsWaste);
-}
-
-/**
- * Get values from long shar.
- *
- * @ingroup fsmmain
- */
-void get_long_sharp_m(double *sharp)
-{
- ROBOT_LOCK(sharpsWaste);
- sharp[0] = robot.sharpsOpponent.longSharpDist1;
- sharp[1] = robot.sharpsOpponent.longSharpDist2;
- sharp[2] = robot.sharpsOpponent.longSharpDist3;
- ROBOT_UNLOCK(sharpsWaste);
-}
-
-/**
- * Check out sharp values to evaluate, if waste is in our reach.
- *
- * @ingroup fsmmain
- * @return returns true if waste is in our reach.
- */
-int waste_ahead()
-{
- int sharp[4];
- int cnt = 0, i;
-
- get_short_sharp_mm(sharp);
- printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
- for (i=0; i<4; i++)
- if (sharp[i] > 0 && sharp[i] < 150)
- cnt++;
-
- return ((cnt >= 1) ? 1 : 0);
-}
-
-/**
- * Check out sharp values to evaluate, if wall is in front of us.
- *
- * @ingroup fsmmain
- * @return returns true if wall is in front of us.
- */
-int wall_ahead()
-{
- int sharp[4];
- int cnt = 0, i;
-
- get_short_sharp_mm(sharp);
- printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
- for (i=0; i<4; i++)
- if (sharp[i] > 0 && sharp[i] < 100)
- cnt++;
-
- return ((cnt >= 2) ? 1 : 0);
-}
-
-/**
- * Detection of the enemy ahead.
- *
- * @return enemy may be ahead us.
- */
-int enemy_ahead()
-{
- double sharp[3];
- int cnt = 0, i;
-
- get_long_sharp_m(sharp);
- printf("long sharp: %f %f %f\n", sharp[0], sharp[1], sharp[2]);
- for (i=0; i<3; i++)
- if (sharp[i] > 0 && sharp[i] < 0.30)
- cnt++;
-
- /* FIXME: dont use enemy avoidance now */
-// return ((cnt >= 1) ? 1 : 0);
- return (0);
-}
-
-/**
- * Check out if stack is empty. Use back innerIR sensors to do it.
- *
- * @return returns true if IR sensors indicate the waste
- */
-int stack_empty()
-{
- int status;
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.back & 0xe0;
- ROBOT_UNLOCK(IRsensors);
- return ((status == 0) ? 1 : 0);
-}
-
-/**
- * Evaluation function to be sure the basket is behind us.
- *
- * @ingroup fsmmain
- * @return
- */
-int basket_behind()
-{
- /* FIXME: return true by now, until the back sharps are connected */
- return 1;
-}
-
-/**
- * Try to detect type of the waste using sensors in the back release space.
- *
- * @ingroup fsmmain
- * @return returns type of the waste.
- */
-int recognize_waste_phase2()
-{
- int type = WASTE_UNKNOWN;
- char bit = 0x01;
- char status;
- int i, cnt = 0;
-
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.back & 0x1f;
- ROBOT_UNLOCK(IRsensors);
-
- for (i=0; i<8; i++)
- if (status & (bit << i))
- cnt++;
-
- switch (cnt) {
- case 0:
- type = WASTE_UNKNOWN;
- break;
- case 1: type = WASTE_BOTTLE;
- break;
- default:
- type = WASTE_CAN;
- break;
- }
-
- return type;
-}
-
-/**
- * Detect on which side of the robot the waste is in front of.
- *
- * @ingroup fsmmain
- * @return Returns enum value LEFT = 0, CENTER = 1, RIGHT = 2
- */
-int short_sharp_direction() {
- int sharp[4];
- int window[3];
- int rv = CENTER;
-
- get_short_sharp_mm(sharp);
- window[0] = sharp[0] + sharp[1];
- window[1] = sharp[1] + sharp[2];
- window[2] = sharp[2] + sharp[3];
-
-// printf("windows: %d %d %d\n",window[0],window[1],window[2]);
-
- if ((window[0] < window[1]) && (window[0] < window[2])) rv = LEFT;
- if ((window[1] <= window[0]) && (window[1] <= window[2])) rv = CENTER;
- if ((window[2] < window[0]) && (window[2] < window[1])) rv = RIGHT;
-
- return rv;
-}
-
-/**
- * Deposite bottle to correct basket according to the actual position and
- * the color of our team.
- */
-void deposite_bottle()
-{
- struct act_pos_type act_pos;
- void (*servo)(void);
-
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
-
-#ifdef WE_ARE_BLUE
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = innerTransporterRight;
- else
- servo = backDoorDown;
-#else
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = innerTransporterLeft;
- else
- servo = backDoorDown;
-#endif
- (*servo)();
-}
-
-/**
- * Deposite can to correct basket according to the actual position and
- * the color of our team.
- */
-void deposite_can()
-{
- struct act_pos_type act_pos;
- void (*servo)();
-
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
-
-#ifdef WE_ARE_BLUE
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = backDoorDown;
- else
- servo = innerTransporterLeft;
-#else
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = backDoorDown;
- else
- servo = innerTransporterRight;
-#endif
- (*servo)();
-}
-
-/**
- * Competition timer. Stop robot when the timer exceeds.
- *
- */
-void *wait_for_end(void *arg)
-{
- sleep(COMPETITION_TIME);
- printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
- robot_exit();
- return NULL;
-}
-
-/**
- * Timer to go to basket.
- *
- */
-void *wait_to_deposition(void *arg)
-{
- sleep(WAIT_FOR_DEPOSITION);
- FSM_SIGNAL(MAIN, EV_SHORT_TIME_TO_END, NULL);
- return NULL;
-}
-
-/**
- * Get position of the point when we know the distance and angle to turn.
- *
- * @param act actual position
- * @param pos countered position
- */
-void get_new_pos(struct act_pos_type *act, struct act_pos_type *pos,
- double l, double phi)
-{
- pos->x = act->x + l*cos(act->phi + phi);
- pos->y = act->y + l*sin(act->phi + phi);
-// pos->phi = act->phi + phi;
- pos->phi = phi;
-}
-
-void robot_goto_point(struct act_pos_type des_pos)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- tc.maxv /= 3;
- robot_trajectory_new(&tc);
- robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
-}
-
-void robot_go_backward_to_point(struct act_pos_type des_pos)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- tc.maxv /= 1;
- robot_trajectory_new_backward(&tc);
- robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, NO_TURN());
-}
-
-/**
- * FSM states start here.
- */
-FSM_STATE_DECL(main_init);
-FSM_STATE_DECL(plan_go_start);
-FSM_STATE_DECL(plan_go);
-FSM_STATE_DECL(align_with_the_wall);
-FSM_STATE_DECL(align_with_the_wall2);
-FSM_STATE_DECL(go_backward_to_nearest_conner);
-FSM_STATE_DECL(go_far_from_conner);
-FSM_STATE_DECL(go_for_waste);
-FSM_STATE_DECL(waste_grab);
-FSM_STATE_DECL(go_backward);
-FSM_STATE_DECL(waste_deposite);
-FSM_STATE_DECL(waste_deposite_wait);
-FSM_STATE_DECL(go_to_basket);
-FSM_STATE_DECL(align_with_the_oppos_wall);
-FSM_STATE_DECL(align_with_the_oppos_wall2);
-FSM_STATE_DECL(go_closer_to_basket);
-FSM_STATE_DECL(go_backward_to_basket);
-FSM_STATE_DECL(search_waste);
-FSM_STATE_DECL(turn_robot);
-FSM_STATE_DECL(follow_waste);
-
-/**
- * Set starting position, playground`s safety zone and other obstacles.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(main_init)
-{
- pthread_t thid;
-
- /* start event ocurred */
- switch (FSM_EVENT) {
-#ifndef WAIT_FOR_START
- case EV_ENTRY:
-#endif
- case EV_START:
- /* start competition timer */
- pthread_create(&thid, NULL, wait_for_end, NULL);
- pthread_create(&thid, NULL, wait_to_deposition, NULL);
- /* init map and shared memory if required */
- ShmapInit(1);
- /* starting position of the robot */
- //robot_set_act_pos_trans(ROBOT_AXIS_TO_BACK_M,
- // PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2,
- // DEG2RAD(0));
- robot_set_act_pos_trans(ROBOT_WIDTH_M/2,
- PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
- DEG2RAD(-90));
- /* safety zone around playground`s wall */
-// ShmapSetRectangleType(0.0, 0.0, 0.1, 2.1, MAP_WALL_CSPACE);
-// ShmapSetRectangleType(0.0, 0.0, 3.0, 0.1, MAP_WALL_CSPACE);
-// ShmapSetRectangleType(0.0, 2.0, 3.0, 2.1, MAP_WALL_CSPACE);
-// ShmapSetRectangleType(2.9, 0.0, 3.0, 2.1, MAP_WALL_CSPACE);
- /* 3 cups for batteries */
-// ShmapSetRectangleFlag(1.3, PLAYGROUND_HEIGHT_M, 1.7,
-// PLAYGROUND_HEIGHT_M - 0.8, MAP_FLAG_WALL, 0);
-// ShmapSetRectangleFlag(1.3, PLAYGROUND_HEIGHT_M/2 - 0.2,
-// 1.7, PLAYGROUND_HEIGHT_M/2 + 0.2,
-// MAP_FLAG_WALL, 0);
- ShmapSetRectangleFlag(1.3, 0.0, 1.7, 0.8, MAP_FLAG_WALL, 0);
- /* waste counter */
- ROBOT_LOCK(waste_cnt);
- robot.waste_cnt = 0;
- printf("waste_cnt = %d\n", robot.waste_cnt);
- ROBOT_UNLOCK(waste_cnt);
- FSM_TIMER(100);
- break;
- case EV_TIMER:
- frontDoorDown();
- /* start to do something */
-#ifdef COMPETITION
- FSM_TRANSITION(plan_go_start);
-#else
- FSM_TRANSITION(plan_go_start);
-// FSM_TRANSITION(plan_go);
-// FSM_TRANSITION(go_to_basket);
-// FSM_TRANSITION(search_waste);
-#endif
- break;
- default: break;
- }
-}
-
-FSM_STATE(plan_go_start)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(NULL);
- robot_trajectory_add_point_trans(ROBOT_WIDTH_M/2, PLAYGROUND_HEIGHT_M - 1.0);
- robot_trajectory_add_point_trans(0.5, PLAYGROUND_HEIGHT_M - 1.0);
- robot_trajectory_add_final_point_trans(0.6, PLAYGROUND_HEIGHT_M -1.0, NO_TURN());
- frontTransporterBackward();
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(search_waste);
- break;
- default: break;
- }
-}
-
-/**
- * Go and try to detect some waste.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(plan_go)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_trans(PLAYGROUND_WIDTH_M - 1.0, 1.0, NO_TURN(), NULL);
- frontDoorDown();
- frontTransporterBackward();
- break;
- case EV_MOTION_DONE:
- printf("target achieved!\n");
- FSM_TRANSITION(search_waste);
- break;
- case EV_GOAL_NOT_REACHABLE:
- DBG("Goal is not reachable: %s\n",fsm_event_str(FSM_EVENT));
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(search_waste);
- break;
- default: break;
- }
-}
-
-/**
- * Try to align robot with the wall
- */
-FSM_STATE(align_with_the_wall)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
- struct act_pos_type act_pos;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- frontDoorUp();
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- tc.maxacc /= 3;
- robot_trajectory_new_backward(&tc);
- robot_trajectory_add_final_point_trans(0.0,
- act_pos.y, NO_TURN());
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(align_with_the_wall2);
- break;
-
- default: break;
- }
-}
-
-FSM_STATE(align_with_the_wall2)
-{
- struct act_pos_type act_pos;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- act_pos.x = TRANS_X(act_pos.x);
- act_pos.y = TRANS_Y(act_pos.y);
- robot_trajectory_new(NULL);
- robot_trajectory_add_final_point_trans(act_pos.x + 0.2,
- act_pos.y,
- TURN_CCW(DEG2RAD(90)));
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(go_backward_to_nearest_conner);
- break;
- default: break;
- }
-}
-
-FSM_STATE(go_backward_to_nearest_conner)
-{
- struct act_pos_type act_pos;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(NULL);
- robot_trajectory_add_final_point_trans(0.2, 0.0,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- act_pos.x = TRANS_X(0.2);
- act_pos.y = TRANS_Y(0);
- act_pos.phi = DEG2RAD(90);
- robot_set_act_pos_notrans(act_pos.x,
- act_pos.y, act_pos.phi);
- FSM_TRANSITION(go_far_from_conner);
- break;
- default: break;
- }
-}
-
-FSM_STATE(go_far_from_conner)
-{
- struct act_pos_type act_pos;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- act_pos.x = TRANS_X(act_pos.x);
- act_pos.y = TRANS_Y(act_pos.y);
- robot_trajectory_new(NULL);
- robot_trajectory_add_final_point_trans(act_pos.x + 0.0,
- act_pos.y + 0.2, TURN_CCW(DEG2RAD(90)));
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(go_to_basket);
- break;
- default: break;
- }
-}
-
-/**
- * Try to search some waste. Detect the position of the waste, if its on the
- * left or on the right side and then turn to that direction to try to detect
- * the waste.
- */
-FSM_STATE(search_waste) {
- struct act_pos_type act_pos;
- static int search_waste_attempts = 0;
- int dir;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
-
- /* FIXME: enemy voidance is not used by now */
- if (enemy_ahead() && enemy_avoidance_attempt_cnt <
- ENEMY_AVOIDANCE_MAX_ATTEMPTS) {
- printf("enemy is ahead of me\n");
- enemy_avoidance_attempt_cnt++;
- get_new_pos(&act_pos, &des_pos, -0.1, DEG2RAD(5));
- robot_goto_point(des_pos);
-
- } else if(waste_ahead()) {
- printf("waste is in front of me\n");
- dir = short_sharp_direction();
- switch(dir) {
- case LEFT:
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(10));
- break;
- case RIGHT:
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(-10));
- break;
- case CENTER:
- default:
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(0));
- break;
- }
- FSM_TRANSITION(go_for_waste);
-
- } else {
- search_waste_attempts++;
- printf("i can't see any waste, sorry\n");
- if (search_waste_attempts >= 4) {
- search_waste_attempts = 0;
- get_new_pos(&act_pos, &des_pos, -0.2, DEG2RAD(10));
- robot_go_backward_to_point(des_pos);
- } else {
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(0));
- robot_goto_point(des_pos);
- }
- /* FIXME: this timer???!!!! */
- FSM_TIMER(5000);
- }
-
- /* FIXME: enemy avoidance is not used by now */
- if (enemy_avoidance_attempt_cnt >= ENEMY_AVOIDANCE_MAX_ATTEMPTS)
- enemy_avoidance_attempt_cnt = 0;
- break;
- case EV_TIMER:
- FSM_TRANSITION(search_waste);
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(search_waste);
- break;
- case EV_SHORT_TIME_TO_END:
- robot_stop();
-#ifdef USE_WALL_TO_ALIGN
- if (no_align)
- FSM_TRANSITION(go_to_basket);
- else
- FSM_TRANSITION(align_with_the_wall);
-#else
- FSM_TRANSITION(go_to_basket);
-#endif
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(go_for_waste)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_point(des_pos);
- break;
- case EV_MOTION_DONE:
- printf("went for waste and finished!!!!!!!!!!!\n");
- FSM_TRANSITION(waste_grab);
- break;
- default: break;
- }
-}
-
-FSM_STATE(waste_grab) {
- static int waste_grab_attempts_cnt = 0;
- int waste_cnt;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_SIGNAL(PICKUP, EV_PICKUP_START, NULL);
- break;
- case EV_PICKUP_READY:
- FSM_SIGNAL(PICKUP, EV_PICKUP_TRY, NULL);
- break;
- case EV_PICKUP_DONE:
- ROBOT_LOCK(waste_cnt);
- robot.waste_cnt++;
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
- if (waste_cnt == 1 && stack_empty()) {
- printf("fsmmain: pickup done. Youuhouuu, I have "
- "a bottle!! In total: %d\n", waste_cnt);
- } else {
- printf("fsmmain: pickup done. But sensors detect "
- "empty stack!! In total: %d\n", waste_cnt);
- }
- /* go to deposite waste if the stack is full */
- if (waste_cnt >= STACK_SIZE) {
-#ifdef USE_WALL_TO_ALIGN
- if (no_align)
- FSM_TRANSITION(go_to_basket);
- else
- FSM_TRANSITION(align_with_the_wall);
-#else
- FSM_TRANSITION(go_to_basket);
-#endif
- /* continue grabbing waste if there`s space in the stack */
- } else {
- FSM_TIMER(NEW_PICK_UP_WAIT);
- }
- break;
- case EV_PICKUP_JAMMED:
- printf("fsmmain: waste jammed\n");
- FSM_TRANSITION(go_backward);
-// FSM_TRANSITION(search_waste);
- break;
- case EV_PICKUP_FAILED:
- printf("fsmmain: pickup failed\n");
- waste_grab_attempts_cnt++;
- if (waste_grab_attempts_cnt >= 2) {
- waste_grab_attempts_cnt = 0;
-// FSM_TRANSITION(go_backward);
- FSM_TRANSITION(search_waste);
- } else {
- FSM_TRANSITION(waste_grab);
- }
- break;
- case EV_TIMER:
- printf("timeout: continue grabbing waste\n");
- FSM_TRANSITION(search_waste);
- break;
- default:
- break;
- }
-}
-
-/**
- * Go backward and try to avoid baterry collection.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(go_backward)
-{
- struct act_pos_type act_pos;
- int dir;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- dir = short_sharp_direction();
- switch(dir) {
- case LEFT:
- get_new_pos(&act_pos, &des_pos, -0.1, DEG2RAD(5));
- break;
- case RIGHT:
- get_new_pos(&act_pos, &des_pos, -0.1, DEG2RAD(-5));
- break;
- case CENTER:
- default:
- get_new_pos(&act_pos, &des_pos, -0.1, DEG2RAD(5));
- break;
- }
- robot_go_backward_to_point(des_pos);
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(search_waste);
- break;
- default: break;
- }
-
-}
-
-/**
- * Go to basket to deposite the waste.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(go_to_basket)
-{
- static int counter;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- frontDoorUp();
- counter = 3;
- case EV_TIMER:
- robot_goto_trans(PLAYGROUND_WIDTH_M - 0.5, 0.5,
- TURN_CW(DEG2RAD(180)), NULL);
- break;
- case EV_MOTION_DONE:
- printf("target achieved! \n");
-// FSM_TRANSITION(go_closer_to_basket);
-// FSM_TRANSITION(go_backward_to_basket);
- FSM_TRANSITION(align_with_the_oppos_wall);
- break;
- case EV_GOAL_NOT_REACHABLE:
- counter--;
- DBG("Goal is not reachable: %s\n",fsm_event_str(FSM_EVENT));
- if (counter)
- FSM_TIMER(1000);
- else
- FSM_TRANSITION(align_with_the_oppos_wall);
-// FSM_TRANSITION(go_backward_to_basket);
- break;
- default: break;
- }
-}
-
-/**
- * Try to align robot with the wall
- */
-FSM_STATE(align_with_the_oppos_wall)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
- struct act_pos_type act_pos;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- tc.maxacc /= 3;
- robot_trajectory_new_backward(&tc);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M,
- act_pos.y, NO_TURN());
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(align_with_the_oppos_wall2);
- break;
-
- default: break;
- }
-}
-
-FSM_STATE(align_with_the_oppos_wall2)
-{
- struct act_pos_type act_pos;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- act_pos.x = TRANS_X(act_pos.x);
- act_pos.y = TRANS_Y(act_pos.y);
- robot_trajectory_new(NULL);
- robot_trajectory_add_final_point_trans(act_pos.x - 0.15,
- act_pos.y, NO_TURN());
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(go_backward_to_basket);
- break;
- default: break;
- }
-}
-
-/**
- * Try to get closer to the wall. It`s useful to enclose the basket.
- */
-FSM_STATE(go_closer_to_basket)
-{
- struct act_pos_type act_pos;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
-
- if (wall_ahead() ||
- goto_basket_attempt_cnt >= GOTO_BASKET_MAX_ATTEMPTS) {
- printf("wall is ahead\n");
- goto_basket_attempt_cnt = 0;
- FSM_TRANSITION(go_backward_to_basket);
- } else {
- goto_basket_attempt_cnt++;
- get_new_pos(&act_pos, &des_pos, 0.05, DEG2RAD(0));
- FSM_TIMER(100);
- robot_goto_point(des_pos);
- }
- break;
- case EV_TIMER:
- FSM_TRANSITION(go_closer_to_basket);
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(go_closer_to_basket);
- break;
- default: break;
- }
-}
-
-/**
- * Go backward to the basket.
- */
-FSM_STATE(go_backward_to_basket)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- tc.maxacc /= 3;
- robot_trajectory_new_backward(&tc);
- robot_trajectory_add_final_point_trans(
- PLAYGROUND_WIDTH_M - 0.15,
- 0.0, NO_TURN());
- break;
- case EV_LOST_DURING_MOTION:
- case EV_MOTION_DONE:
- /* FIXME: we should check if the position of robot
- * is really closed to the basket */
- FSM_TRANSITION(waste_deposite);
- break;
- default: break;
- }
-}
-
-/**
- * Deposite waste.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(waste_deposite)
-{
- int waste_cnt, type;
-
- ROBOT_LOCK(waste_cnt);
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
-
- if (stack_empty() && waste_cnt != 0) {
- printf("waste_cnt is %d, but I detect stack empty!!!\n",
- waste_cnt);
- }
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- printf("deposite waste: %d\n", waste_cnt);
- innerDoorDown();
- FSM_TIMER(WASTE_ROLL_OUT_WAIT);
- break;
- case EV_TIMER:
- innerDoorUp();
- type = recognize_waste_phase2();
- printf("waste recognition: ");
- switch (type) {
- case WASTE_CAN:
- printf("it`s a can\n");
- deposite_can();
- break;
- case WASTE_BOTTLE:
- printf("it`s a bottle\n");
- deposite_bottle();
- break;
- default:
- deposite_bottle();
- printf("unidentified\n");
- break;
- }
- /* need to recognize type of waste here */
- FSM_TRANSITION(waste_deposite_wait);
- break;
- default: break;
- }
-}
-
-/**
- * Wait until deposition terminated then go to find more waste.
- *
- */
-FSM_STATE(waste_deposite_wait)
-{
- int waste_cnt;
- switch(FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(waste_cnt);
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
- if (waste_cnt > 1)
- FSM_TIMER(DEPOSITE_WAIT);
- else
- FSM_TIMER(LAST_DEPOSITE_WAIT);
- break;
- case EV_TIMER:
- ROBOT_LOCK(waste_cnt);
- if (robot.waste_cnt > 0)
- robot.waste_cnt--;
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
- innerTransporterStop();
- backDoorUp();
- if (waste_cnt > 0) {
- FSM_TRANSITION(waste_deposite);
- } else {
- robot_set_act_pos_trans(PLAYGROUND_WIDTH_M - 0.1,
- 0.1, DEG2RAD(90));
- FSM_TRANSITION(plan_go);
- no_align = 1;
- }
- break;
- default: break;
- }
-}
-
-/* main loop */
-int main()
-{
- /* robot initialization */
- robot_init();
-
- FSM(MAIN)->debug_states = 1;
-
- robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
- robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
-// robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
-// robot.fsm[FSM_ID_DET].state = &fsm_state_det_start;
- robot.fsm[FSM_ID_PICKUP].state = &fsm_state_pickup_init;
-
- /* start threads and wait */
- robot_start();
-
- robot_wait();
-
- /* clean up */
- robot_destroy();
-
- return 0;
-}
+++ /dev/null
-/**
- * @file fsmmove.cc
- * @brief FSM move
- * @author Michal Sojka, Jose Maria Martin Laguna
- *
- */
-#define FSM_MOTION
-#include <sys/time.h>
-#include <time.h>
-#include "trgen.h"
-#include "balet.h"
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <pthread.h>
-#include <path_planner.h>
-#include <signal.h>
-#include <movehelper_eb2007.h>
-#include <sharp.h>
-#include <unistd.h>
-#include <map.h>
-#include "robot_config.h"
-#include <robomath.h>
-
-
-/*******************************************************************************
- * Parameters of Obstacle detection
- *******************************************************************************/
-
-#define OBS_PERIOD_WATCH 100000 /**< Time period for watching the sensors in us*/
-#define OBS_PERIOD_BETWEEN_DET 1000000 /**< Minimal time period between two detections in us. */
-#define OBS_AREA (200/MAP_CELL_SIZE_MM) /**< Configuration Space of the obstacle in cell units. In this case 200mm = 2 cells. @warning It should be an integer */
-#define OBS_AREA_M (OBS_AREA*MAP_CELL_SIZE_MM)
-#define OBS_FORGET_PERIOD 100000 /**< The period of thread_update_map() */
-
-#define OBS_FORGET_SEC 15 /**< Time to completely forget detected obstacle. */
-
-#define OBS_OFFSET 0.6
-/*******************************************************************************
- * Controller thread and helper functions for that thread
- *******************************************************************************/
-
-
-//Controller gains
-const struct balet_params k = {
- p_tangent: 2, // dx gain
- p_angle: 2, // dphi gain
- p_perpen: 10 // dy gain
-};
-#define MAX_POS_ERROR_M 0.5
-#define CLOSE_TO_TARGET_M 0.1
-#define MAX_WAIT_FOR_CLOSE_MS 2000
-
-//#define MOTION_PERIOD_NS (500/*ms*/*1000*1000)
-#define MOTION_PERIOD_NS (50/*ms*/*1000*1000)
-#define SIG_TIMER (SIGRTMIN+0)
-#define SIG_NEWDATA (SIGRTMIN+1)
-
-// Global varibles
-static pthread_t thr_trajctory_follower;
-static pthread_t thr_trajctory_recalc;
-static pthread_t thr_update_map;
-static struct timeval tv_start; /**< Absolute time, when trajectory started. */
-
-/** Stores the actually followed trajectory object */
-static Trajectory *actual_trajectory = NULL;
-
-//static Trajectory *switch_to_trajectory = NULL;
-//static double switch_time;
-
-// function definition
-static int new_goal(void);
-static void stop();
-
-bool actual_trajectory_reverse;
-pthread_mutex_t actual_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
-pthread_mutex_t switch_to_trajectory_lock = PTHREAD_MUTEX_INITIALIZER;
-
-// static void odometry(double l, r) {
-// static double last_l, last_r;
-
-// robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000;
-// robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2)/1000;
-// robot.act_pos.phi = 0;
-
-// ROBOT_LOCK(act_pos);
-// ROBOT_UNLOCK(act_pos);
-// }
-
-static void delete_actual_trajectory()
-{
- Trajectory *old;
- pthread_mutex_lock(&actual_trajectory_lock);
- old = actual_trajectory;
- actual_trajectory = NULL;
- pthread_mutex_unlock(&actual_trajectory_lock);
- robot_send_speed(0,0);
- if (old) delete(actual_trajectory);
-}
-
-/** Sends events from follower thread to FSM. */
-static void notify_fsm(bool done, double error)
-{
- static bool done_sent;
-
- if (error > MAX_POS_ERROR_M) {
- FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
- } else {
- if (done) {
- if (error < CLOSE_TO_TARGET_M) {
- FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
- } else if (!done_sent) {
- done_sent = true;
- FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
- }
- } else {
- done_sent = false;
- }
- }
-}
-
-/**
- * A thread running the controller.
- *
- * This (high priority) thread executes the motion control
- * algorithm. It calculates repference position based on actual
- * trajectory and current time. Then it calls "balet" controller to
- * close feedback.
- *
- * @param arg
- *
- * @return
- */
-static void *trajectory_follower(void *arg)
-{
- struct itimerspec ts;
- sigset_t sigset;
- struct sigevent sigevent;
- timer_t timer;
-
- ts.it_value.tv_sec = 0;
- ts.it_value.tv_nsec = MOTION_PERIOD_NS;
- ts.it_interval.tv_sec = 0;
- ts.it_interval.tv_nsec = MOTION_PERIOD_NS;
-
- sigemptyset(&sigset);
- sigaddset(&sigset, SIG_TIMER);
- sigaddset(&sigset, SIG_NEWDATA);
-
- pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*) NULL);
-
- sigevent.sigev_notify = SIGEV_SIGNAL;
- sigevent.sigev_signo = SIG_TIMER;
- sigevent.sigev_value.sival_int = 0;
-
- if (timer_create(CLOCK_REALTIME, &sigevent, &timer) < 0) {
- perror("move: timer_create");
- return NULL;
- }
-
- if (timer_settime(timer, 0, &ts, NULL) < 0) {
- perror("move: timer_settimer");
- return NULL;
- }
-
- while (1) {
- double speeds[2];
- int sig;
- // Wait for start of new period or new data
- sigwait(&sigset, &sig);
- //DBG("signal %d ", sig - SIGRTMIN);
- pthread_mutex_lock(&actual_trajectory_lock);
- Trajectory *w = actual_trajectory;
- if (w) {
- Pos ref_pos, act_pos, balet_out;
- bool done;
- double t;
- struct timeval tv;
-
- // Calculate reference position
- gettimeofday(&tv, NULL);
- t = (double)(tv.tv_usec - tv_start.tv_usec) / 1000000.0;
- t += (tv.tv_sec - tv_start.tv_sec);
-
-
- done = w->getRefPos(t, ref_pos);
-
-#ifdef CONFIG_OPEN_LOOP
- // We don't use any feedback now. It is
- // supposed that the actual position is equal
- // to reference position.
- robot_set_act_pos_notrans(ref_pos.x, ref_pos.y, ref_pos.phi);
-#endif
- ROBOT_LOCK(ref_pos);
- ref_pos.x = robot.ref_pos.x;
- ref_pos.y = robot.ref_pos.y;
- ref_pos.phi = robot.ref_pos.phi;
- ROBOT_UNLOCK(ref_pos);
-
- DBG("rx=%5.02f ry=%5.02f, rphi=%4.0f v=%-4.02f omega=%-4.02f\r", ref_pos.x, ref_pos.y, ref_pos.phi/M_PI*180, ref_pos.v, ref_pos.omega);
- fflush(stdout);
- // Call the controller
- double error;
- error = balet(ref_pos, ref_pos, k, balet_out);
- speeds[0] = balet_out.v - ROBOT_ROTATION_RADIUS_M*balet_out.omega;
- speeds[1] = balet_out.v + ROBOT_ROTATION_RADIUS_M*balet_out.omega;
- notify_fsm(done, error);
- } else {
- speeds[0] = 0;
- speeds[1] = 0;
- }
-
-
- // Apply controller output
- robot_send_speed(speeds[0], speeds[1]);
- pthread_mutex_unlock(&actual_trajectory_lock);
- }
-}
-
-static void obstacle_detected_at(double x, double y)
-{
- int i,j, xcell, ycell;
- struct ref_pos_type ref_pos;
- struct map *map = ShmapIsMapInit();
- double xx, yy;
- bool obs_already_seen;
-
- ShmapPoint2Cell(x, y, &xcell, &ycell);
- if (!ShmapIsCellInMap(xcell, ycell))
- return;
- obs_already_seen = (map->cells[ycell][xcell].flags & (MAP_FLAG_DET_OBST)) != 0;
-
- DBG("New obstacle detected at cell [%f,%f] \n", x,y);
- bool needs_replan = false; // This flag indicates if the path should be recalculated
- bool obstacle_at_goal = false; // This flag indicates that the goal is an obstacle
- //DBG("Cell %d, %d\n", xcell, ycell);
-
- /** Then all the cells arround obstacle cell are set as
- * #MAP_NEW_OBSTACLE. Cells of current robot position are not
- * set to avoid path planning deadlock. If there are a path
- * cell between them, the path will be recalculated. @see
- * #OBS_CSPACE. */
-
- ROBOT_LOCK(ref_pos);
- ref_pos = robot.ref_pos;
- ROBOT_UNLOCK(ref_pos);
-
- for (i=(xcell-OBS_AREA); i <= xcell+OBS_AREA; i++) {
- for (j=(ycell- OBS_AREA); j <=ycell + OBS_AREA; j++) {
- if (!ShmapIsCellInMap(i, j)) continue;
- ShmapCell2Point(i, j, &xx, &yy);
- if (distance(xx, yy, ref_pos.x, ref_pos.y) > 0.2) {
- if (i==xcell && j==ycell)
- map->cells[ycell][xcell].flags |= MAP_FLAG_DET_OBST;
- map->cells[j][i].detected_obstacle = MAP_NEW_OBSTACLE;
- if (map->cells[j][i].flags & MAP_FLAG_PATH) needs_replan = true;
- if (map->cells[j][i].flags & MAP_FLAG_GOAL) obstacle_at_goal = true;
- }
- }
- }
-
- // Path recalculation if the goal is not an obstacle and the path should be recalc
- if (obstacle_at_goal) {
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- }
- else if (needs_replan && !obs_already_seen) {
- if (new_goal() == 0)
- DBG("Path recalc\n");
- else {
- // TODO:
- }
- }
-}
-
-/**
- * A thread running the trajectory recalc
- *
- * This (low-medium priority) thread updates the map with sensors information.
- * If it is necesary, it recalculate the path.
- *
- * @param arg
- *
- * @return
- */
-static void * trajctory_recalc(void * arg)
-{
-#define NUM_SHARPS 3
- double val[NUM_SHARPS], x, y;
- //Pos p;
- struct ref_pos_type p;
- int i;
-
- while (1) {
- /** Updates Map. */
- //ShmapUpdateTmpObstacles(OBS_UPDATE_MAP_SPEED);
- ROBOT_LOCK(sharpsOpponent);
- val[0] = robot.sharpsOpponent.longSharpDist1;
- val[1] = robot.sharpsOpponent.longSharpDist2;
- val[2] = robot.sharpsOpponent.longSharpDist3;
- ROBOT_UNLOCK(sharpsOpponent);
-
- const float ang[NUM_SHARPS] = {DEG2RAD(22), DEG2RAD(0), DEG2RAD(-22)};
- for (i=0; i<NUM_SHARPS; i++) {
- if(val[i] < OBS_OFFSET) {
- /** Get actual position. */
- ROBOT_LOCK(ref_pos);
- p = robot.ref_pos;
- ROBOT_UNLOCK(ref_pos);
- /**
- * The obstacle cell its calulated as (val*cos(p.phi)) + p.x) and ((val*sin(p.phi)) + p.y)
- * where p is actual position and val is the value of the sensors in milimiters. Then ShmapPoint2Cell_X() and
- * ShmapPoint2Cell_Y() funcions are used to get cell coordonate.
- */
- val[i] += ROBOT_AXIS_TO_BELT_M;
- x = val[i]*cos(p.phi+ang[i]) + p.x;
- y = val[i]*sin(p.phi+ang[i]) + p.y;
- obstacle_detected_at(x, y);
-
- //waits between detection
- usleep(OBS_PERIOD_BETWEEN_DET);
- }
- }
-
- /** This process repeats with a period of #OBS_PERIOD_WATCH. */
- usleep(OBS_PERIOD_WATCH);
- }
-}
-
-/**
- * A thread updating the map
- */
-static void * thread_update_map(void * arg)
-{
- int val = (long long)MAP_NEW_OBSTACLE/(OBS_FORGET_SEC*1000000/OBS_FORGET_PERIOD);
- if (val == 0) val = 1;
- while (1) {
- /** Updates Map. */
- ShmapUpdateTmpObstacles(val);
- usleep(OBS_FORGET_PERIOD);
- }
-}
-
-/**
- * Tells trajctory_follower to start moving along trajectory @c t.
- *
- * @param t Trajectory to follow.
- */
-static void go(Trajectory *t)
-{
- Trajectory *old;
- pthread_mutex_lock(&actual_trajectory_lock);
- old = actual_trajectory;
- gettimeofday(&tv_start, NULL);
- actual_trajectory = t;
- pthread_mutex_unlock(&actual_trajectory_lock);
- if (old)
- delete(old);
-}
-
-static void stop()
-{
- delete_actual_trajectory();
- pthread_kill(thr_trajctory_follower, SIG_NEWDATA);
-}
-
-
-/**
- * @brief Calculates and simplify a path to goal position avoiding obstacles. In case of error it sends the proper event to MAIN FSM.
- * @return Zero on succes, non-zero on error.
- * @note Only call this function from this file.
- */
-static int new_goal(void)
-{
- struct ref_pos_type goal;
- double startx, starty, angle;
- PathPoint * path = NULL;
- PathPoint * tmp_point = NULL;
-
- struct TrajectoryConstraints tc;
- Pos start;
- //tc.maxv *= 2.5;
- //tc.maxacc *= 2.5;
-
- /// Get actual position.
- ROBOT_LOCK(ref_pos);
- startx = robot.ref_pos.x;
- starty = robot.ref_pos.y;
- start.x = startx;
- start.y = starty;
- start.phi = robot.ref_pos.phi;
- tc = robot.goal_trajectory_constraints;
- ROBOT_UNLOCK(ref_pos);
- /// Get goal position.
- ROBOT_LOCK(goal);
- goal = robot.goal;
- ROBOT_UNLOCK(goal);
-
- //DBG("Going from (%.2f, %.2f) to (%.2f, %.2f)\n", startx, starty, goalx, goaly);
- /// Calculates the path between actual position to goal position. If the goal is not reachable returns -1.
- switch (path_planner(startx, starty, goal.x, goal.y, &path, &angle)) {
- case 1:
- break;
- case PP_ERROR_MAP_NOT_INIT:
- printf("ERROR: Map not initialized\n");
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- return -1;
- break;
- case PP_ERROR_GOAL_IS_OBSTACLE:
- printf("ERROR: Goal is obstacle\n");
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- return -1;
- break;
- case PP_ERROR_GOAL_NOT_REACHABLE:
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
- FSM_SIGNAL(MAIN, EV_GOAL_NOT_REACHABLE, NULL);
- return -1;
- break;
- default:
- break;
- // shouldn't we call freePathMemory() here? No, because no memory has been allocanted for the path, path is only a pointer to NULL
- }
-
- bool backward = false;
- Trajectory *t = new Trajectory(tc, backward);
-
- // Add this path to the trajectory.
- for (tmp_point = path; tmp_point!=NULL; tmp_point=tmp_point->next) {
- //DBG("ADDDING POINT (%f, %f)\n", tmp_point->x, tmp_point->y);
- t->addPoint(tmp_point->x, tmp_point->y);
- }
-
- // add the path to the robot trajectory
- t->finalHeading = robot.goal_final_heading; // NAN should be OK here, if not, it is error.
-
- // Free path allocated memory
- freePathMemory(path);
-
- if (t->prepare(start)) {
- go(t);
- } else {
- DBG("ERROR preparing trajectory\n");
- delete(t);
- FSM_SIGNAL(MAIN, EV_TRAJECTORY_ERROR, NULL);
- return -1;
- }
-
- return 0;
-}
-
-
-/**
- * Starts moving on trajectory prepared by functions in
- * movehelper.cc. In case of error it sends the proper event to MAIN
- * FSM.
- *
- * @return Zero on succes, non-zero on error.
- */
-static int new_trajectory(void)
-{
- Trajectory *t;
- Pos pos;
- ROBOT_LOCK(new_trajectory);
- t = (Trajectory*) robot.new_trajectory;
- if (t) robot.new_trajectory = NULL;
- else {
- ROBOT_UNLOCK(new_trajectory);
- DBG("No trajectory\n");
- FSM_SIGNAL(MAIN, EV_TRAJECTORY_ERROR, NULL);
- return -1;
- }
- ROBOT_UNLOCK(new_trajectory);
-
-
- ROBOT_LOCK(est_pos);
- pos.x = robot.est_pos.x;
- pos.y = robot.est_pos.y;
- pos.phi = robot.est_pos.phi;
- ROBOT_UNLOCK(est_pos);
-
- if (t->prepare(pos)) {
- go(t);
- } else {
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); // FIXME: Is this correct?
- return -1;
- }
- return 0;
-}
-
-/*******************************************************************************
- * The automaton
- *******************************************************************************/
-
-FSM_STATE_DECL(movement);
-FSM_STATE_DECL(close_to_target);
-FSM_STATE_DECL(wait_for_command);
-
-
-FSM_STATE(move_init)
-{
- pthread_attr_t tattr;
- sched_param param;
- int ret;
-
- if (FSM_EVENT == EV_ENTRY) {
- actual_trajectory = NULL;
-
- // Trajectory follower thread
- pthread_attr_init (&tattr);
- pthread_attr_getschedparam (&tattr, ¶m);
- param.sched_priority = TRAJ_FOLLOWER_PRIO;
- ret = pthread_attr_setschedparam (&tattr, ¶m);
- if(ret) {
- perror("pthread_attr_setschedparam");
- exit(1);
- }
- ret = pthread_create(&thr_trajctory_follower, &tattr, trajectory_follower, NULL);
- if(ret) {
- perror("pthread_create");
- exit(1);
- }
- // Thread of trajectory recalc
- pthread_attr_init (&tattr);
- pthread_attr_getschedparam (&tattr, ¶m);
- param.sched_priority = TRAJ_RECALC_PRIO;
- ret = pthread_attr_setschedparam (&tattr, ¶m);
- if(ret) {
- perror("pthread_attr_setschedparam");
- exit(1);
- }
- pthread_create(&thr_trajctory_recalc, NULL, trajctory_recalc, NULL);
-
- // Thread update Map
- pthread_attr_init (&tattr);
- pthread_attr_getschedparam (&tattr, ¶m);
- param.sched_priority = UPDATE_MAP_PRIO;
- ret = pthread_attr_setschedparam (&tattr, ¶m);
- if(ret) {
- perror("pthread_attr_setschedparam");
- exit(1);
- }
- pthread_create(&thr_update_map, NULL, thread_update_map, NULL);
-
- FSM_TRANSITION(wait_for_command);
- }
-}
-
-FSM_STATE(wait_for_command)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- stop();
- break;
- case EV_NEW_GOAL:
- if (new_goal() != 0) {
- // The event must be the return value of new_goal()
- DBG("Goal error\n");
- break;
- }
- FSM_TRANSITION(movement);
- break;
- case EV_NEW_TRAJECTORY:
- if (new_trajectory() != 0) {
- break;
- }
- FSM_TRANSITION(movement);
- break;
- default:
- break;
- }
-}
-
-
-
-FSM_STATE(movement)
-{
- switch (FSM_EVENT) {
- case EV_TRAJECTORY_DONE:
- FSM_TRANSITION(close_to_target);
- break;
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_TRAJECTORY_LOST:
- FSM_SIGNAL(MAIN, EV_LOST_DURING_MOTION, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_MOVE_STOP:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_EXIT:
- stop();
- break;
- default:
- // TODO: Macro FSM_DEFAULT prints an ungandled event
- break;
- }
-}
-
-
-FSM_STATE(close_to_target)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(MAX_WAIT_FOR_CLOSE_MS);
- break;
- case EV_TRAJECTORY_DONE_AND_CLOSE:
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_TRAJECTORY_LOST:
- case EV_TIMER:
- stop();
- FSM_SIGNAL(MAIN, EV_LOST_DURING_MOTION, NULL);
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_MOVE_STOP:
- FSM_TRANSITION(wait_for_command);
- break;
- case EV_EXIT:
- stop();
- break;
- case EV_TRAJECTORY_DONE:
- case EV_NEW_TRAJECTORY:
- case EV_LOC_UPDATED:
- case EV_NEW_GOAL:
- case EV_LOC_LOST:
- case EV_RETURN:
- DBG_PRINT_EVENT("unhandled event");
- break;
- }
-}
+++ /dev/null
-#define FSM_PICKUP
-
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <fsm.h>
-#include <sharp.h>
-#include <servos_eb2007.h>
-
-/* bad attempts counter. pickup tries to pickup until the limit is exceeded */
-int pickup_bad_attemp_cnt = 0;
-#define PICKUP_MAX_BAD_ATTEMPTS 3
-
-/**
- * Time constants
- */
-/* time to wait for the waste rolling up to the stack. If time exceeded, the
- * attempt is considered as jammed */
-#define WASTE_ROLL_IN_WAIT 2000
-/* time to wait for waste disengagement during the jam */
-#define WASTE_DISENGAGEMENT_WAIT 2500
-/* time period between unsuccessful attempts */
-#define PICK_UP_TRY_PERIOD 1000
-
-
-/**
- * Convert sharp`s measured values to mm.
- *
- * @ingroup fsmpickup
- * @param sharp Sharp values in mm
- */
-void get_short_sharp_mm(int *sharp)
-{
- ROBOT_LOCK(sharpsWaste);
- sharp[0] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short1);
- sharp[1] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short2);
- sharp[2] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short3);
- sharp[3] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short4);
- ROBOT_UNLOCK(sharpsWaste);
-}
-
-/**
- * Evaluation function for waste pickuping.
- *
- * @ingroup fsmpickup
- * @return return true if waste entered inside the entry.
- */
-int waste_entered()
-{
- int sharp[4];
- int cnt = 0, i;
-
- get_short_sharp_mm(sharp);
- printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
- for (i=0; i<4; i++)
- if (sharp[i] > 1 && sharp[i] < 40)
- cnt++;
-
- return ((cnt >= 1) ? 1 : 0);
-}
-
-/**
- * Sharp evaluation funtion.
- *
- * @ingroup fsmpickup
- * @return Returns true if sharps are covered. This means the front
- * door is closed.
- */
-int short_sharp_enclosed()
-{
- int sharp[4];
- int cnt = 0, i;
-
- get_short_sharp_mm(sharp);
- printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
- for (i=0; i<4; i++)
- if (sharp[i] < 20)
- cnt++;
-
- return ((cnt >= 3) ? 1 : 0);
-}
-
-/**
- * Evaluation function to detect waste jammed went pinking up.
- *
- * @ingroup fsmpickup
- * @return Returns true when the door is unable to closed fully.
- */
-int front_door_jammed()
-{
- int rv;
-
- ROBOT_LOCK(frontDoorAngle);
- rv = (robot.frontDoorAngle.state > 0x220) ? 0 : 1;
- ROBOT_UNLOCK(frontDoorAngle);
-
- return rv;
-}
-
-/**
- * Try to detect type of the waste using sensors at the front door.
- *
- * @return waste type
- */
-int recognize_waste_phase1()
-{
- char status;
- int front_door_angle;
- int type = WASTE_UNKNOWN;
- int i, cnt = 0;
- char bit = 0x01;
-
- ROBOT_LOCK(frontDoorAngle);
- front_door_angle = robot.frontDoorAngle.state;
- ROBOT_UNLOCK(frontDoorAngle);
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.front;
- ROBOT_UNLOCK(IRsensors);
-
- if (front_door_angle > 0x220)
- return type;
-
- for (i=0; i<8; i++)
- if (status & (bit << i))
- cnt++;
-
- switch (cnt) {
- case 0:
- type = WASTE_UNKNOWN;
- break;
- case 1:
- case 2: type = WASTE_BOTTLE;
- break;
- default:
- type = WASTE_CAN;
- break;
- }
-
- return type;
-}
-
-/**
- * FSM states start here.
- */
-FSM_STATE_DECL(pickup_wait);
-FSM_STATE_DECL(pickup_try);
-FSM_STATE_DECL(waste_recognition);
-FSM_STATE_DECL(pickup_wait_for_success);
-FSM_STATE_DECL(jam_avoidance);
-FSM_STATE_DECL(pickup_jam);
-
-/**
- * FSM State: Pickup state machine initialization. Release front rolling belt
- * and release front door down.
- *
- * @ingroup fsmpickup
- */
-FSM_STATE(pickup_init)
-{
- switch (FSM_EVENT) {
- default:
- releaseFront();
- //frontDoorDown();
- FSM_TIMER(1500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(pickup_wait);
- break;
- }
-}
-
-/**
- * FSM State: Pickup main waiting loop. Wait for events from main FSM.
- *
- * @ingroup fsmpickup
- */
-FSM_STATE(pickup_wait) {
- switch (FSM_EVENT) {
- case EV_ENTRY:
- /*printf("fsmpickup ready to serve\n");*/
- break;
- case EV_PICKUP_START:
- printf("start picking up waste\n");
- pickup_bad_attemp_cnt = 0;
- frontTransporterForward();
- FSM_SIGNAL(MAIN, EV_PICKUP_READY, NULL);
- break;
- case EV_PICKUP_TRY:
- /*printf("try to pick up a waste\n");*/
- FSM_TRANSITION(pickup_try);
- break;
- default: break;
- }
-}
-
-/**
- * Wait until the waste entered the front and try to close front door to
- * pick up the it. If maximal attempt exceeds, picking up is considered as
- * unsuccessful. In other way try to pick up again.
- *
- * @ingroup fsmpickup
- */
-FSM_STATE(pickup_try)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- case EV_TIMER:
- if (waste_entered()) {
- printf("waste entered: continue!!!!!!!!!\n");
- frontDoorUp();
- FSM_TRANSITION(pickup_wait_for_success);
- } else {
- pickup_bad_attemp_cnt++;
- printf("waste not entered: bad attempts: %d\n",
- pickup_bad_attemp_cnt);
- if (pickup_bad_attemp_cnt >= PICKUP_MAX_BAD_ATTEMPTS) {
- FSM_SIGNAL(MAIN, EV_PICKUP_FAILED, NULL);
- FSM_TRANSITION(pickup_wait);
- } else {
- FSM_TIMER(PICK_UP_TRY_PERIOD);
- }
- }
- break;
- default: break;
- }
-}
-
-/**
- * Try to identify the type of waste with front sensors.
- *
- * @ingroup fsmpickup
- */
-FSM_STATE(waste_recognition)
-{
- int type;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(50);
- break;
- case EV_TIMER:
- type = recognize_waste_phase1();
- switch (type) {
- case WASTE_CAN:
- printf("it`s a can\n");
- break;
- case WASTE_BOTTLE:
- printf("it`s a bottle\n");
- break;
- default:
- printf("unidentified\n");
- break;
- }
- FSM_TRANSITION(pickup_wait_for_success);
- break;
- default: break;
- }
-}
-
-/**
- * Front door is closed up, so the waste should goes up. If the door can`t be
- * closed up fully means, that waste is jammed so try to release it out.
- *
- * @ingroup fsmpickup
- */
-FSM_STATE(pickup_wait_for_success) {
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(WASTE_ROLL_IN_WAIT);
- break;
- case EV_TIMER:
- if(short_sharp_enclosed()
- || !front_door_jammed()) {
- /*printf("waste pick up done\n");*/
- frontDoorDown();
- FSM_TRANSITION(jam_avoidance);
- } else {
- /*printf("waste pick up failed\n");*/
- FSM_TRANSITION(pickup_jam);
- }
- break;
- default: break;
- }
-}
-
-/**
- * Avoid waste jammed under the front door when trying to pick up the waste.
- */
-FSM_STATE(jam_avoidance)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- frontTransporterBackward();
- FSM_TIMER(400);
- break;
- case EV_TIMER:
- FSM_TRANSITION(pickup_wait);
- FSM_SIGNAL(MAIN, EV_PICKUP_DONE, NULL);
- break;
- default: break;
- }
-}
-
-/**
- * Try to release the waste from the jammed state.
- *
- * @ingroup fsmpickup
- */
-FSM_STATE(pickup_jam) {
- switch (FSM_EVENT) {
- case EV_ENTRY:
- printf("trying to released jammed waste\n");
- frontDoorDown();
- frontTransporterBackward();
- FSM_TIMER(WASTE_DISENGAGEMENT_WAIT);
- break;
- case EV_TIMER:
- /*printf("waste release: done\n");*/
- frontTransporterStop();
- FSM_SIGNAL(MAIN, EV_PICKUP_JAMMED, NULL);
- FSM_TRANSITION(pickup_wait);
- break;
- default: break;
- }
-}
+++ /dev/null
-/**
- * @file movehelper.cc
- * @author Michal Sojka
- * @date Wed Jun 13 13:27:58 2007
- *
- * @brief Conveniency functions for motion control.
- *
- *
- */
-
-#include "trgen.h"
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <movehelper_eb2007.h>
-#include <path_simplifier.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/stat.h>
-#include <math.h>
-#include <map.h>
-#include <robomath.h>
-
-#if 1
-struct TrajectoryConstraints trajectoryConstraintsDefault = {
- maxv: 0.5, // m/s
- maxomega: 1.5, // rad/s
- maxangacc: 2, // rad/s^2
- maxacc: 0.6, // m/s^2
- maxcenacc: 1, // m/s^2
- maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
-};
-#else
-struct TrajectoryConstraints trajectoryConstraintsDefault = {
- maxv: 0.9, // m/s
- maxomega: 4.5, // rad/s
- maxangacc: 3*1, // rad/s^2
- maxacc: 3*0.3, // m/s^2
- maxcenacc: 3*1, // m/s^2
- maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
-};
-#endif
-
-/**
- *
- *
- *
- */
-static void robot_trajectory_new_ex(struct TrajectoryConstraints *tc, bool backward)
-{
- Trajectory *ot, *nt;
-
- if (!tc) tc = &trajectoryConstraintsDefault;
-
- nt = new Trajectory(*tc, backward);
- ROBOT_LOCK(new_trajectory);
- ot = (Trajectory*)robot.new_trajectory;
- robot.new_trajectory = nt;
-
- ROBOT_UNLOCK(new_trajectory);
- if (ot) delete(ot);
-}
-
-
-/**
- * Initializes new trajectory object for adding points. Path planner
- * will not be used.
- *
- * @param tc Constrains for the trajectory.
- */
-void robot_trajectory_new(struct TrajectoryConstraints *tc) {
- robot_trajectory_new_ex(tc, false);
-}
-void robot_trajectory_new_backward(struct TrajectoryConstraints *tc) {
- robot_trajectory_new_ex(tc, true);
-}
-
-/** Sets actual position of the robot and with respoect to color of
- * the team. Should be used for setting initial position of the
- * robot. */
-void robot_set_act_pos_notrans(double x, double y, double phi)
-{
- if (x<0) x=0;
- if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
- if (y<0) y=0;
- if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
- ROBOT_LOCK(est_pos);
- robot.est_pos.x = x;
- robot.est_pos.y = y;
- robot.est_pos.phi = phi;
- ROBOT_UNLOCK(est_pos);
-}
-
-/**
- * Adds point in absolute coordinates to previously initialized trajectory object.
- *
- * @param x_m X coordinate in meters.
- * @param y_m Y coordinate in meters.
- */
-void robot_trajectory_add_point_notrans(double x_m, double y_m)
-{
- Trajectory *w;
-
- ROBOT_LOCK(new_trajectory);
- w = (Trajectory*)robot.new_trajectory;
- if (w) {
- w->addPoint(x_m, y_m);
- }
- ROBOT_UNLOCK(new_trajectory);
-}
-
-double target_x, target_y;
-
-/**
- * Adds final point to trajectory objects and starts robot movement.
- *
- * @param x_m X coordinate in meters.
- * @param y_m Y coordinate in meters.
- * @param heading Desired heading (in degrees) of the robot in this
- * point. NAN means don't care. Positive number or zero
- * means turn counter-clockwise, negative number means
- * turn clockwise. Example: DEG2RAD(90) means turn up
- * counter-clockwise and DEG2RAD(-270) means turn up
- * clockwise.
- */
-void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct final_heading *heading)
-{
- target_x = x_m;
- target_y = y_m;
- robot_trajectory_add_point_notrans(target_x, target_y);
-
- Trajectory *w;
- ROBOT_LOCK(new_trajectory);
- w = (Trajectory*)robot.new_trajectory;
- if (w) {
- w->finalHeading = *heading;
- }
- ROBOT_UNLOCK(new_trajectory);
-
- FSM_SIGNAL(MOTION, EV_NEW_TRAJECTORY, NULL);
-}
-
-
-/**
- * Stops actual movement.
- *
- */
-void robot_stop()
-{
- FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
-}
-
-
-void robot_send_speed(double left, double right) {
- double mul;
- unsigned l, r;
-
- mul = 1000.0 / ROBOT_WHEEL_RADIUS_MM; // to angular speed
- mul *= 28.0/(2.0*M_PI)/*gear*/*(1<<8); // to pxmc speed
-
- // I hope it is not neccessary to lock here
- l = (int)(left * mul);
- r = (int)(right * mul);
- robot.orte_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
- robot.orte_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
-// DBG("speed l=%8.3g (%6d) r=%8.3g (%6d)",
-// left, l, // robot.orte_speed.left,
-// right, r //robot.orte_speed.right
-// );
- ORTEPublicationSend(robot.publisherMotor);
-}
-
-/**
- * Makes move the robot to a target position. Use path panner to find
- * the trajectory. This function is intended to be called from main
- * FSM.
- *
- * @param x X coordinate in meters.
- * @param y Y coordinate in meters.
- * @param heading Desired heading (in degrees) of the robot at goal
- * point. NAN means don't care. Positive number or zero
- * means turn counter-clockwise, negative number means
- * turn clockwise. Example: DEG2RAD(90) means turn up
- * counter-clockwise and DEG2RAD(-270) means turn up
- * clockwise.
- */
-void robot_goto_notrans(double x, double y, struct final_heading *heading, struct TrajectoryConstraints *tc)
-{
- ROBOT_LOCK(goal);
- robot.goal.x = x;
- robot.goal.y = y;
- robot.goal_final_heading = *heading;
- if (tc) robot.goal_trajectory_constraints = *tc;
- else robot.goal_trajectory_constraints = trajectoryConstraintsDefault;
- ROBOT_UNLOCK(goal);
- FSM_SIGNAL(MOTION, EV_NEW_GOAL, NULL);
-}
+++ /dev/null
-#ifndef MOVEHELPER_EB2007_H
-#define MOVEHELPER_EB2007_H
-
-#include <robodata.h>
-#include <robot_eb2007.h>
-#include <trgenconstr.h>
-#include <math.h>
-
-extern struct TrajectoryConstraints trajectoryConstraintsDefault;
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* extern double target_x, target_y; */
-
-#if defined(WE_ARE_RED) && defined(WE_ARE_BLUE)
-#error Both colors defined
-#endif
-
-#if !defined(WE_ARE_RED) && !defined(WE_ARE_BLUE)
-#error No colors defined
-#endif
-
-#ifdef WE_ARE_RED
-static inline double __trans_ang(double phi)
-{
- double a;
- a = M_PI/2.0 - phi + M_PI/2.0;
- a = fmod(a, 2.0*M_PI);
- if (phi >= 0 && a >= 0) a -= 2.0*M_PI;
- if (phi < 0 && a < 0) a += 2.0*M_PI;
- return a;
-}
-static inline struct final_heading* __trans_heading(struct final_heading *h)
-{
- if (h->turn_type != FH_DONT_TURN) {
- h->heading = __trans_ang(h->heading);
- switch (h->turn_type) {
- case FH_CW:
- h->turn_type = FH_CCW;
- break;
- case FH_CCW:
- h->turn_type = FH_CW;
- break;
- default:
- break;
- }
- }
- return h;
-}
-
-#define TRANS_X(x) (PLAYGROUND_WIDTH_M - (x))
-#define TRANS_Y(y) (y)
-#define TRANS_ANG(a) __trans_ang(a)
-#define TRANS_HEADING(h) __trans_heading(h)
-#endif
-
-#ifdef WE_ARE_BLUE
-#define TRANS_X(x) (x)
-#define TRANS_Y(y) (y)
-#define TRANS_ANG(a) (a)
-#define TRANS_HEADING(h) (h)
-#endif
-
-
-
-void robot_set_act_pos_notrans(double x, double y, double phi);
-#define robot_set_act_pos_trans(x, y, phi) robot_set_act_pos_notrans(TRANS_X(x), TRANS_Y(y), TRANS_ANG(phi))
-
-void robot_trajectory_new(struct TrajectoryConstraints *tc);
-void robot_trajectory_new_backward(struct TrajectoryConstraints *tc);
-
-void robot_trajectory_add_point_notrans(double x_m, double y_m);
-#define robot_trajectory_add_point_trans(x, y) robot_trajectory_add_point_notrans(TRANS_X(x), TRANS_Y(y))
-
-void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct final_heading *heading);
-#define robot_trajectory_add_final_point_trans(x, y, heading) robot_trajectory_add_final_point_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading))
-
-void robot_stop();
-
-void robot_send_speed(double left, double right);
-
-void robot_goto_notrans(double x, double y, struct final_heading *heading, struct TrajectoryConstraints *tc);
-#define robot_goto_trans(x, y, heading, tc) robot_goto_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading), tc)
-
-static inline void robot_translate_coordinates(double *x, double *y, double *phi)
-{
- if (x) {
- *x = TRANS_X(*x);
- }
- if (phi) {
- *phi = TRANS_ANG(*phi);
- }
-}
-
-
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif /* MOVEHELPER_EB2007_H */
+++ /dev/null
-events = {
- "main" : {
- "EV_START" : "Competition started.",
- "EV_SHORT_TIME_TO_END" : "Time to go for the deposition regarless of what do we have.",
- "EV_COLLECT_WASTE" : "",
- "EV_DEPOSITE_BOTTLE" : "",
- "EV_DEPOSITE_CAN" : "",
- "EV_DEPOSITE_DONE" : "",
- "EV_WAIT_DEPOSITE_BOTTLE" : "",
- "EV_WAIT_DEPOSITE_CAN" : "",
- "EV_STACK_FULL" : "",
-
- "EV_MOTION_DONE" : "Previously submitted motion is finished",
- "EV_LOST_DURING_MOTION" : "Actual position of the robot is too far from reference",
- "EV_TRAJECTORY_ERROR" : "The trajectory submitted by ::EV_NEW_TRAJECTORY can't be prepared for execution.",
-
- "EV_GOAL_NOT_REACHABLE" : "Path planner can't calculate a path to the goal. Probably obstacles are on the way.",
-
- "EV_DETECTED_BOTTLE" : "",
- "EV_DETECTED_CAN" : "",
- "EV_DETECTED_BATTERY" : "",
- "EV_DETECTED_WASTE" : "",
- "EV_OBSTRUCTION_AHEAD" : "",
- "EV_ENEMY_AHEAD" : "",
-
- "EV_PICKUP_READY" : "Pickup FSM is ready to try pickup waste.",
- "EV_PICKUP_DONE" : "Pickup FSM sucessfully moved the waste to stock.",
- "EV_PICKUP_FAILED" : "Pick up failed.",
- "EV_PICKUP_JAMMED" : "Waste seems to be jammed.",
- },
- "detect" : {
- },
- "loc" : {
- "EV_LOC_INIT" : "",
- "EV_LOC_LOCATE" : "",
- "EV_ODOMETRY_RECEIVED" : "",
- "EV_LASER_RECEIVED" : "",
- "EV_POSITION_UPDATED" : "",
- },
- "pickup" : {
- "EV_PICKUP_START" : "",
- "EV_TRASH_GOES_UP" : "",
- "EV_PICKUP_TRY" : "",
- "EV_PICKUP_FREE" : "",
- },
- "motion" : {
- "EV_MOVE_STOP" : "Stop current motion as fast as possible",
- "EV_NEW_TRAJECTORY" : "New trajectory submitted to robot.new_trajectory",
- "EV_NEW_GOAL" : "A new goal is received",
- "EV_TRAJECTORY_LOST" : "Actual position of the robot is too far from reference",
- "EV_TRAJECTORY_DONE" : "Reference position is at the end of the previously submitted trajectory",
- "EV_TRAJECTORY_DONE_AND_CLOSE" : "::EV_TRAJECTORY_DONE + distance of actual position to the target is less than XXX",
- "EV_LOC_UPDATED" : "Localization updated the estimate of the robot position",
- "EV_LOC_LOST" : "Localization can't estimate the position of the robot.",
- },
- "joystick" : {
- },
- "display" : {
- "EV_SWITCH_TO_CONS" : "Switch display mode to CONSOLE",
- "EV_SWITCH_TO_STATUS" : "Switch display mode to STATUS",
- "EV_SWITCH_TO_MAP" : "Switch display mode to MAP",
- "EV_SWITCH_TO_MISC" : "Switch display mode to MISC",
- }
-}
+++ /dev/null
-#include <orte.h>
-#include <movehelper_eb2007.h>
-
-#ifdef CONFIG_ORTE_RTL
-#include <linux/module.h>
-#include <posix/pthread.h>
-#define printf rtl_printf
-#elif defined CONFIG_ORTE_RTAI
-#include <linux/module.h>
-#include <rtai/compat.h>
-#define printf rt_printk
-#else
-#include <stdio.h>
-#endif
-
-#include <robot_eb2007.h>
-#include <robottype.h>
-#include <robottype_eb2007.h>
-#include <math.h>
-#include <string.h>
-
-ORTEDomain *d=NULL;
-double k0 = 0;
-double k1 = 0;
-double rozvor = 0.255; // rozvor - zjistit
-double aktUhel = 0;
-int firstRun = 1;
-struct motion_irc_type initPos;
-
-struct sharpShorts_type sharpWaste;
-struct sharpLongs_type sharpOpponent;
-struct stateInnerIR_type IRsensors;
-
-static void recvCallBackLaser(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- struct laserData_type *instance=(struct laserData_type*)vinstance;
-
- switch (info->status) {
- case NEW_DATA:
- ROBOT_LOCK(laser_recv);
- //robot.laser_recv.cnt = instance->cnt;
- //robot.laser_recv.period = ((struct laserData_type *)vinstance)->measures[0];
- //memcpy(&robot.laser_recv.measures, instance->measures, sizeof(short)*10);
- robot.laser_recv = *instance;
- //printf("laser data received\n");
- ROBOT_UNLOCK(laser_recv);
- FSM_SIGNAL(LOC, EV_LASER_RECEIVED, NULL);
- break;
- case DEADLINE:
- printf("laser data deadline\n");
- //DBG("laser deadline occurred\n");
- break;
- }
-}
-
-static void *subscriberLaserCreate(void *arg) {
- //static struct laserData_type orte_laserData;
-
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- laserData_type_register(d);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "laser", "laserData",
- &robot.laser_recv, &deadline, &minimumSeparation,
- recvCallBackLaser, NULL, IPADDRESS_INVALID);
- return arg;
-}
-
-static void *subscriberAngleCreate(void *arg) {
- //static struct laserData_type orte_laserData;
-
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- stateFrontDoor_type_register(d);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "FD", "stateFrontDoor",
- &robot.frontDoorAngle, &deadline, &minimumSeparation,
- NULL, NULL, IPADDRESS_INVALID);
- return arg;
-}
-
-static void recvSharp1CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- //struct sharpLongs_type *instance=(struct sharpLongs_type*)vinstance;
-
- switch (info->status) {
- case NEW_DATA:
- ROBOT_LOCK(sharpsOpponent);
- robot.sharpsOpponent = sharpOpponent;
- ROBOT_UNLOCK(sharpsOpponent);
- //DBG("Callback\n");
- //fsm_signal(FSM(LOC), EV_LASER_RECEIVED);
- break;
- case DEADLINE:
- //DBG("long sharps deadline occurred\n");
- break;
- }
-}
-
-static void recvSharp2CallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- //struct sharpShorts_type *instance=(struct sharpShorts_type*)vinstance;
-
- switch (info->status) {
- case NEW_DATA:
- ROBOT_LOCK(sharpsWaste);
- robot.sharpsWaste = sharpWaste;
- ROBOT_UNLOCK(sharpsWaste);
-
- //fsm_signal(FSM(LOC), EV_LASER_RECEIVED);
- break;
- case DEADLINE:
- //DBG("short sharps deadline occurred\n");
- break;
- }
-}
-
-static void recvIRCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- //struct stateInnerIR_type *instance=(struct stateInnerIR_type*)vinstance;
-
- switch (info->status) {
- case NEW_DATA:
- ROBOT_LOCK(IRsensors);
- robot.IRsensors = IRsensors;
- ROBOT_UNLOCK(IRsensors);
-
- //fsm_signal(FSM(LOC), EV_LASER_RECEIVED);
- break;
- case DEADLINE:
- //DBG("IR deadline occurred\n");
- break;
- }
-}
-
-static void recvOdoCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- struct motion_irc_type *instance=(struct motion_irc_type*)vinstance;
- static struct motion_irc_type prevInstance;
- static int firstRun = 1;
- double n = (double)(28.0 / 1.0); // spocitat prevodovy pomer
- double c = (M_PI*2*ROBOT_WHEEL_RADIUS_MM/1000) / (n * 4*500.0); // vzdalenost na pulz - 4.442 je empiricka konstanta :)
-
- double aktk0 = 0;
- double aktk1 = 0;
- double deltaU = 0;
- double dk0 = 0;
- double dk1 = 0;
- double deltAlfa = 0;
-
- switch (info->status) {
- case NEW_DATA:
- robot.odometry = *instance;
- if(firstRun) {
- prevInstance = *instance;
- firstRun = 0;
- break;
- }
- else {
- robot.odometry.left -= initPos.left;
- robot.odometry.right -= initPos.right;
- }
-
- aktk0 = (prevInstance.left - instance->left) >> 8;
- aktk1 = (instance->right - prevInstance.right) >> 8;
- prevInstance = *instance;
- dk0 = aktk0;
- dk1 = aktk1;
-
- k0 = aktk0;
- k1 = aktk1;
-
- dk0 *= c;
- dk1 *= c;
- //printf("dk.left=%f dk.right=%f\n",dk0,dk1);
- deltaU = (dk0 + dk1) / 2;
-
- deltAlfa = (dk1 - dk0) / (2.0*ROBOT_ROTATION_RADIUS_MM/1000); // dk1 - dk0
- /* FIXME */
-/* ROBOT_LOCK(act_pos); */
-/* aktUhel = robot.act_pos.phi; */
-/* ROBOT_UNLOCK(act_pos); */
-
- aktUhel += deltAlfa; // aktualni uhel
-
- double x, y, phi;
-/* ROBOT_LOCK(act_pos); */
-/* x = robot.act_pos.x + deltaU * cos(aktUhel); */
-/* y = robot.act_pos.y + deltaU * sin(aktUhel); */
-/* phi = robot.act_pos.phi = aktUhel; */
-/* ROBOT_UNLOCK(act_pos); */
- robot_set_act_pos_notrans(x, y, phi);
- break;
- case DEADLINE:
- break;
- }
-}
-
-static void recvStartCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- struct stateDigIn_type *instance=(struct stateDigIn_type*)vinstance;
- static int lastState = 0;
- switch (info->status) {
- case NEW_DATA:
- if((instance->state==0) && (lastState==0)) //ZAVOLAM STARTOVACI FUNKCI;
- lastState=1;
- robot.start = 0;
- if((instance->state==1) && (lastState==1)) //ZAVOLAM STARTOVACI FUNKCI;
- {
- lastState = 2;
- robot.start = 1;
- FSM_SIGNAL(MAIN, EV_START, NULL);
- }
- //fsm_signal(FSM(LOC), EV_LASER_RECEIVED);
- break;
- case DEADLINE:
- //DBG("IR deadline occurred\n");
- break;
- }
-}
-
-static void recvJoyCallBack(const ORTERecvInfo *info,void *vinstance, void *recvCallBackParam) {
- struct joy_data_type *instance=(struct joy_data_type*)vinstance;
-
- switch (info->status) {
- case NEW_DATA:
- ROBOT_LOCK(joy);
- robot.joy = *instance;
- ROBOT_UNLOCK(joy);
-
- //fsm_signal(FSM(LOC), EV_LASER_RECEIVED);
- break;
- case DEADLINE:
- //DBG("IR deadline occurred\n");
- break;
- }
-}
-
-void *subscriberJoyCreate(void *arg) {
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- joy_data_type_register(d);
-// ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv));
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "joy", "joy_data",
- &(robot.joy), &deadline, &minimumSeparation, recvJoyCallBack, arg, IPADDRESS_INVALID);
- return arg;
-}
-
-void *subscriberIRCreate(void *arg) {
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- stateInnerIR_type_register(d);
-// ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv));
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "IR", "stateInnerIR", &IRsensors, &deadline, &minimumSeparation, recvIRCallBack, arg, IPADDRESS_INVALID);
- return arg;
-}
-
-void *subscriberOdoCreate(void *arg) {
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- motion_irc_type_register(d);
-// ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv));
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "position", "motion_irc", &(robot.odometry), &deadline, &minimumSeparation, recvOdoCallBack, arg, IPADDRESS_INVALID);
- return arg;
-}
-
-void *subscriberDICreate(void *arg) {
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- stateDigIn_type_register(d);
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "DI", "stateDigIn", &(robot.startBit), &deadline, &minimumSeparation, recvStartCallBack, arg, IPADDRESS_INVALID);
- return arg;
-}
-
-void *subscriberSharp1Create(void *arg) {
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- sharpLongs_type_register(d);
-// ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv));
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "sharpSouper", "sharpLongs",
- &sharpOpponent, &deadline, &minimumSeparation, recvSharp1CallBack, arg, IPADDRESS_INVALID);
- //FIXME: ^ Orte to prepisuje? Neni to lockovany!!!!
- return arg;
-}
-
-void *subscriberSharp2Create(void *arg) {
- ORTESubscription *s;
- NtpTime deadline,minimumSeparation;
-
- sharpShorts_type_register(d);
-// ORTETypeRegisterAdd(d,"SharpMsg1",NULL,NULL,NULL,sizeof(sharpToRecv));
- NTPTIME_BUILD(deadline,10);
- NTPTIME_BUILD(minimumSeparation,0);
- s=ORTESubscriptionCreate(d, IMMEDIATE, BEST_EFFORTS, "sharpLahve", "sharpShorts", &sharpWaste, &deadline, &minimumSeparation, recvSharp2CallBack, arg, IPADDRESS_INVALID);
- return arg;
-}
-
-void sendCallBackMotor(const ORTESendInfo *info,void *instance, void *sendCallBackParam) {
- switch (info->status) {
- case NEED_DATA:
- //DBG("need data\n");
- break;
- case CQL:
- //DBG("cql\n");
- break;
- }
-}
-static void *publisherMotorCreate(void *arg) {
- NtpTime persistence, delay;
-
- motion_speed_type_register(d);
-
- NTPTIME_BUILD(persistence,3);
- NTPTIME_BUILD(delay,1);
- robot.publisherMotor=ORTEPublicationCreate(d, "motor", "motion_speed", &robot.orte_speed, &persistence, 2, sendCallBackMotor, NULL, &delay);
- return arg;
-}
-
-void sendCallBackServo(const ORTESendInfo *info,void *vinstance, void *sendCallBackParam) {
- //struct stateServa_type *instance=(struct stateServa_type*)vinstance;
-
- switch (info->status) {
- case NEED_DATA:
- break;
- case CQL: //criticalQueueLevel
- break;
- }
-}
-
-void *publisherCreateServo(void *arg) {
- NtpTime persistence, delay;
-
- stateServa_type_register(d);
- NTPTIME_BUILD(persistence,3);
- NTPTIME_BUILD(delay,1);
- robot.publisherServa=ORTEPublicationCreate(d, "serva", "stateServa", &(robot.serva), &persistence, 2, sendCallBackServo, arg, &delay);
- return arg;
-}
-
-void sendCallBackPosition(const ORTESendInfo *info,void *vinstance, void *sendCallBackParam) {
- act_pos *instance=(act_pos*)vinstance;
-
- switch (info->status) {
- case NEED_DATA:
- instance = &(robot.act_pos);
- break;
- case CQL: //criticalQueueLevel
- break;
- }
-}
-
-void *publisherCreatePosition(void *arg) {
- NtpTime persistence, delay;
-
- act_pos_type_register(d);
- NTPTIME_BUILD(persistence,3);
- NtpTimeAssembFromMs(delay,0,40);
- robot.publisherPosition=ORTEPublicationCreate(d, "pos", "act_pos", &(robot.act_pos), &persistence, 2, sendCallBackPosition, arg, &delay);
- return arg;
-}
-
-void sendCallBackEstPosition(const ORTESendInfo *info,void *vinstance, void *sendCallBackParam) {
- est_pos *instance=(est_pos*)vinstance;
-
- switch (info->status) {
- case NEED_DATA:
- ROBOT_LOCK(est_pos);
- instance = &(robot.est_pos);
- ROBOT_UNLOCK(est_pos);
- break;
- case CQL: //criticalQueueLevel
- break;
- }
-}
-
-void *publisherCreateEstPosition(void *arg) {
- NtpTime persistence, delay;
-
- est_pos_type_register(d);
- NTPTIME_BUILD(persistence,3);
- NtpTimeAssembFromMs(delay,0,40);
- robot.publisherEstPosition=ORTEPublicationCreate(d, "est_pos", "est_pos", &(robot.est_pos), &persistence, 2, sendCallBackEstPosition, arg, &delay);
- return arg;
-}
-
-void sendCallBackAccu(const ORTESendInfo *info,void *vinstance, void *sendCallBackParam) {
-
- switch (info->status) {
- case NEED_DATA:
- break;
- case CQL: //criticalQueueLevel
- break;
- }
-}
-
-void *publisherCreateAccu(void *arg) {
- NtpTime persistence, delay;
-
- accumulator_type_register(d);
- NTPTIME_BUILD(persistence,3);
- NTPTIME_BUILD(delay,1);
- robot.publisherAccumulator=ORTEPublicationCreate(d, "accu", "accumulator", &(robot.accu), &persistence, 2, sendCallBackAccu, arg, &delay);
- return arg;
-}
-
-void initOrte() {
- ORTEInit();
-
- ORTEVerbositySetOptions("ALL.0");
- d=ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN,NULL,NULL,ORTE_FALSE);
- if (!d) {
- printf("ORTEDomainAppCreate failed!\n");
- }
- subscriberJoyCreate(NULL);
- subscriberLaserCreate(NULL);
- publisherMotorCreate(NULL);
- publisherCreateServo(NULL);
- subscriberSharp1Create(NULL);
- subscriberSharp2Create(NULL);
- subscriberIRCreate(NULL);
- subscriberDICreate(NULL);
- subscriberOdoCreate(NULL);
- subscriberAngleCreate(NULL);
- publisherCreatePosition(NULL);
- publisherCreateEstPosition(NULL);
-}
-
-void terminateOrte() {
- ORTEDomainAppDestroy(d);
-}
+++ /dev/null
-#include <robot_eb2007.h>
-#include <signal.h>
-#include <unistd.h>
-#include <servos_eb2007.h>
-#include <movehelper_eb2007.h>
-
-static int thread_priorities[] = {
- [FSM_ID_MOTION] = 25,
- [FSM_ID_MAIN] = 20,
- [FSM_ID_DET] = 15,
- [FSM_ID_JOY] = 15,
- [FSM_ID_LOC] = 10
-};
-
-static char *fsm_name[] = {
- [FSM_ID_MOTION] = "motion",
- [FSM_ID_MAIN] = "main",
- [FSM_ID_DET] = "detection",
- [FSM_ID_JOY] = "joystick",
- [FSM_ID_LOC] = "localization",
- [FSM_ID_PICKUP] = "pickup"
-};
-
-struct robot_eb2007 robot;
-
-static void block_signals()
-{
- sigset_t sigset;
- int i;
-
- sigemptyset(&sigset);
- for (i=0; i<7; i++)
- sigaddset(&sigset, SIGRTMIN+i);
-
- pthread_sigmask(SIG_BLOCK, &sigset, (sigset_t*)NULL);
-}
-
-static void int_handler(int sig)
-{
- robot_exit();
-}
-
-int fsm_det_in(struct robo_fsm *fsm, int ret_code);
-
-/**
- * Initializes the robot.
- * Setup fields in robot structure, initializes FSMs and ORTE.
- *
- * @return 0
- */
-int robot_init()
-{
- int i;
- robot.mode = ROBO_TESTING;
-
- pthread_mutex_init(&robot.lock, NULL);
- pthread_mutex_init(&robot.lock_act_pos, NULL);
-
- /* FSM initialization */
- for (i=0; i<FSM_CNT; i++)
- fsm_init(&robot.fsm[i], fsm_name[i]);
-
-#ifdef WE_ARE_RED
- printf("We are RED!\n");
-#endif
-#ifdef WE_ARE_BLUE
- printf("We are BLUE!\n");
-#endif
-
- signal(SIGINT, int_handler);
- block_signals();
-
- robot.serva.release = FRONT_LAUNCH_LOCK;
- robot.serva.frontDoor = FRONT_DOOR_UP;
- robot.serva.innerDoor = INNER_DOOR_UP;
- robot.serva.backDoor = BACK_DOOR_UP;
- robot.serva.transporterFront = FRONT_TRANSPORTER_STOP;
- robot.serva.transporterInner = INNER_TRANSPORTER_STOP;
-
- robot.sharpsOpponent.longSharpDist1 = 2.0;
- robot.sharpsOpponent.longSharpDist2 = 2.0;
- robot.sharpsOpponent.longSharpDist3 = 2.0;
-
- initOrte();
- ORTEPublicationSend(robot.publisherServa);
-
- return 0;
-}
-/**
- * Starts the robot threads.
- * @todo Don't exit() in case of error.
- * @return 0
- */
-int robot_start()
-{
- int i;
-
- /* start FSM threads */
- for(i=0; i<FSM_CNT; i++) {
- pthread_attr_t attr;
- struct sched_param param;
-
- pthread_attr_init(&attr);
-
- param.sched_priority = thread_priorities[i];
- pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
- if (pthread_attr_setschedparam(&attr, ¶m)) {
- perror("pthread_attr_setschedparam");
- exit(1);
- }
-
-
- if (fsm_start(&robot.fsm[i], &attr) != 0) {
- perror("fsm_start");
- exit(1);
- }
- }
-
- return 0;
-}
-
-/**
- * Signals all the robot threads to finish.
- * @return 0
- */
-int robot_exit()
-{
- int i;
- /* start FSM threads */
- for(i=0; i<FSM_CNT; i++) {
- fsm_event e = { EV_EXIT, NULL };
- __fsm_signal(&robot.fsm[i], e, FSM_WAIT_FOREVER);
- }
- return 0;
-}
-
-/**
- * Wait for finishing the robot threads after calling robot_exit(). *
- *
- * @return 0
- */
-int robot_wait()
-{
- int i;
- /* wait for threads */
- for (i=0; i<FSM_CNT; i++) {
- pthread_join(robot.fsm[i].threadid, NULL);
- }
- return 0;
-}
-
-/**
- * Stops the robot. All resources alocated by robot_init() or
- * robot_start() are dealocated here.
-
- * @return 0
- */
-int robot_destroy()
-{
- int i;
- printf("\n\n\n");
- for (i=0;i<3;i++) { /* For better reliability :( */
- frontDoorDown();
- frontDoorUp();
- innerDoorUp();
- backDoorUp();
- frontTransporterStop();
- innerTransporterStop();
- lockFront();
- usleep(100000);
- }
-
- for (i=0; i<FSM_CNT; i++)
- fsm_destroy(&robot.fsm[i]);
- DBG("robofsm: stop.\n");
- terminateOrte();
- return 0;
-}
-
+++ /dev/null
-/**
- * robot_eb2007.h 07/04/18
- *
- * Robot's data structures for the Eurobot 2007.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#ifndef ROBOT_EB2007_H
-#define ROBOT_EB2007_H
-
-#include <stdint.h>
-#include <stdio.h>
-#include <mcl.h>
-#include <trgenconstr.h>
-#include <robottype.h>
-#include <robottype_eb2007.h>
-#include <robodim_eb2007.h>
-#include <roboevent_eb2007.h>
-#include <fsm.h>
-#include <servos_eb2007.h>
-
-/* Select robot color. If we are BLUE, no coordinate transformation will
- * be performed. */
-// #define WE_ARE_RED
-#define WE_ARE_BLUE
-
-/**
- * FSM
- */
-/* amount of FSM */
-#define FSM_CNT 6
-
-/* FSM ids */
-enum robot_fsm_id {
- FSM_ID_MAIN = 0,
- FSM_ID_DET = 1,
- FSM_ID_LOC = 2,
- FSM_ID_MOTION = 3,
- FSM_ID_PICKUP = 4,
- FSM_ID_JOY = 5
-};
-
-#define LAS_CNT 12 /* dont forget to change CRA!! */
-/* index of information in times array */
-#define LAS_MEAS_CNTI 0 /* counter index of measured times */
-#define LAS_MEAS_PERI 1 /* period index */
-#define LAS_MEAS_DATAI 2 /* data index */
-
-enum {
- WASTE_BOTTLE,
- WASTE_CAN,
- WASTE_UNKNOWN
-};
-
-enum bottle_sens_state {
- NOTHING = 0,
- FAR,
- CLOSE
-};
-
-struct bottle_sens {
- uint8_t sens1;
- uint8_t sens2;
- uint8_t sens3;
- uint8_t sens4;
-};
-
-/* Mapping of robot structure fields to locks */
-//#define __robot_lock_ lock /* ROBOT_LOCK() */
-#define __robot_lock_ref_pos lock_ref_pos
-#define __robot_lock_est_pos lock
-#define __robot_lock_des_pos lock
-#define __robot_lock_goal lock
-#define __robot_lock_IRsensors lock
-#define __robot_lock_joy lock
-#define __robot_lock_laser_recv lock
-#define __robot_lock_new_trajectory lock
-#define __robot_lock_sharpsOpponent lock
-#define __robot_lock_sharpsWaste lock
-#define __robot_lock_waste_cnt lock
-#define __robot_lock_frontDoorAngle lock
-
-/* robot's main data structure */
-struct robot_eb2007 {
- long long cnt;
- unsigned char mode;
- pthread_mutex_t lock;
- pthread_mutex_t lock_ref_pos;
-
- /** Temporary storage for new trajectory. After the trajectory creation is
- * finished, this trajectory is submitted to fsmmove. */
- void *new_trajectory;
-
- unsigned char isTrajectory;
- unsigned char start;
-
- /* partial FSMs */
- struct robo_fsm fsm[FSM_CNT];
-
- /* mcl model */
- struct mcl_model mcl;
- /* actual position */
- struct ref_pos_type ref_pos;
-
- /* estimated position */
- struct est_pos_type est_pos;
- /* goal position */
- struct ref_pos_type goal;
- struct TrajectoryConstraints goal_trajectory_constraints;
- struct final_heading goal_final_heading;
- /* move */
- struct ref_pos_type move_dpos;
-
- /* orte */
- struct laserData_type laser_recv;
- struct motion_speed_type orte_speed;
-
- struct stateServa_type serva;
- struct sharpShorts_type sharpsWaste;
- struct sharpLongs_type sharpsOpponent;
- struct stateInnerIR_type IRsensors;
- struct stateDigIn_type startBit;
- struct stateFrontDoor_type frontDoorAngle;
-
- struct bottle_sens bott_sens;
-
- struct motion_irc_type odometry;
-
- struct accumulator_type accu;
-
- struct joy_data_type joy;
-
- ORTEPublication *publisherMotor;
- ORTEPublication *publisherServa;
- ORTEPublication *publisherPosition;
- ORTEPublication *publisherEstPosition;
- ORTEPublication *publisherAccumulator;
-
- int bottle_under_belt;
- int waste_cnt;
-}; /* robot_eb2007 */
-
-extern struct robot_eb2007 robot;
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void initOrte();
-void terminateOrte();
-int robot_init();
-int robot_start();
-int robot_exit();
-int robot_wait();
-int robot_destroy();
-
-FSM_STATE_DECL(main_init);
-FSM_STATE_DECL(pickup_init);
-FSM_STATE_DECL(move_init);
-FSM_STATE_DECL(det_start);
-FSM_STATE_DECL(loc_init);
-FSM_STATE_DECL(joy_init);
-
-#ifdef __cplusplus
-}
-#endif
-
-/*Thread priorities*/
-#define TRAJ_FOLLOWER_PRIO 255 /* As high as possible */
-#define OBST_FORGETING_PRIOTR 10 /* Priority of the thread for forgeting detected obstacles. */
-#define TRAJ_RECALC_PRIO 0
-
-#endif /* ROBOT_EB2007_H */
+++ /dev/null
-/*
- * servos_eb2007.c 07/04/18
- *
- * Robot's servos control.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#include <robot_eb2007.h>
-#include <servos_eb2007.h>
-
-void releaseFront(void) {
- robot.serva.release = FRONT_LAUNCH_OPEN;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void lockFront(void) {
- robot.serva.release = FRONT_LAUNCH_LOCK;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void frontDoorUp(void) {
- robot.serva.frontDoor = FRONT_DOOR_UP;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void frontDoorDown(void) {
- robot.serva.frontDoor = FRONT_DOOR_DOWN;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void innerDoorUp(void) {
- robot.serva.innerDoor = INNER_DOOR_UP;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void innerDoorDown(void) {
- robot.serva.innerDoor = INNER_DOOR_DOWN;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void backDoorUp(void) {
- robot.serva.backDoor = BACK_DOOR_UP;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void backDoorDown(void) {
- robot.serva.backDoor = BACK_DOOR_DOWN;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void frontTransporterForward(void) {
- robot.serva.transporterFront = FRONT_TRANSPORTER_FORWARD;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void frontTransporterForwardFast(void) {
- robot.serva.transporterFront = FRONT_TRANSPORTER_FORWARD_FAST;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void frontTransporterStop(void) {
- robot.serva.transporterFront = FRONT_TRANSPORTER_STOP;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void frontTransporterBackward(void) {
- robot.serva.transporterFront = FRONT_TRANSPORTER_BACKWARD;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void innerTransporterLeft(void) {
- robot.serva.transporterInner = INNER_TRANSPORTER_LEFT;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void innerTransporterRight(void) {
- robot.serva.transporterInner = INNER_TRANSPORTER_RIGHT;
- ORTEPublicationSend(robot.publisherServa);
-}
-
-void innerTransporterStop(void) {
- robot.serva.transporterInner = INNER_TRANSPORTER_STOP;
- ORTEPublicationSend(robot.publisherServa);
-}
-
+++ /dev/null
-/*
- * servos_eb2007.h 07/04/18
- *
- * Robot's servos control.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#ifndef SERVOS_EB2007_H
-#define SERVOS_EB2007_H
-
-#define FRONT_DOOR_UP 20
-#define FRONT_DOOR_DOWN 251
-#define INNER_DOOR_UP 255
-#define INNER_DOOR_DOWN 1
-#define BACK_DOOR_UP 160
-#define BACK_DOOR_DOWN 1
-#define TRANSPORTER_OFF 127
-#define FRONT_LAUNCH_LOCK 195
-#define FRONT_LAUNCH_OPEN 66
-#define FRONT_TRANSPORTER_FORWARD (127+60)
-#define FRONT_TRANSPORTER_FORWARD_FAST (127+60)
-#define FRONT_TRANSPORTER_STOP 127
-#define FRONT_TRANSPORTER_BACKWARD (127-20)
-#define INNER_TRANSPORTER_LEFT (127-26)
-#define INNER_TRANSPORTER_STOP 127
-#define INNER_TRANSPORTER_RIGHT (127+16)
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-void releaseFront(void);
-void lockFront(void);
-void frontDoorUp(void);
-void frontDoorDown(void);
-void innerDoorUp(void);
-void innerDoorDown(void);
-void backDoorUp(void);
-void backDoorDown(void);
-void frontTransporterForward(void);
-void frontTransporterForwardFast(void);
-void frontTransporterBackward(void);
-void frontTransporterStop(void);
-void innerTransporterLeft(void);
-void innerTransporterStop(void);
-void innerTransporterRight(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* SERVOS_EB2007_H */
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-
-test_PROGRAMS += follower
-follower_SOURCES = follower.cc
-
-test_PROGRAMS += testpickup
-testpickup_SOURCES = testpickup.cc
-
-test_PROGRAMS += testcollavoid
-testcollavoid_SOURCES = testcollavoid.cc
-
-# Libraries linked to all programs in this Makefile
-lib_LOADLIBES = robot_eb2007 mcl laser-nav robomath robottype robottype_eb2007 pthread \
- rt m orte pathplan sharp map fsm rbtree motion
-
+++ /dev/null
-/*
- * follower.cc 07/10/01
- *
- * Test waste tracking down.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_MAIN
-#include <sharp.h>
-#include <robomath.h>
-#include <robot_eb2007.h>
-#include <movehelper_eb2007.h>
-#include <trgen.h>
-#include <servos_eb2007.h>
-#include <map.h>
-
-#define HOMOLOGACE
-/**
- * Time constants.
- */
-/* time before stopping the deposition belt resp. closing the back door */
-#define DEPOSITE_WAIT 1000
-/* the same as above, but we need to wait a little bit longer for the last one */
-#define LAST_DEPOSITE_WAIT 1500
-/* time before closing the inner door */
-#define WASTE_ROLL_OUT_WAIT 900
-/* time before another pickup */
-#define NEW_PICK_UP_WAIT 1000
-/* waste detection period */
-#define WASTE_DETECTION_PERIOD 100
-
-struct ref_pos_type des_pos;
-
-enum {
- UNKNOWN = 0,
- BOTTLE,
- CAN
-};
-
-/**
- * Convert sharp`s measured values to mm.
- *
- * @ingroup fsmmain
- */
-void get_short_sharp_mm(int *sharp)
-{
- ROBOT_LOCK(sharpsWaste);
- sharp[0] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short1);
- sharp[1] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short2);
- sharp[2] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short3);
- sharp[3] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short4);
- ROBOT_UNLOCK(sharpsWaste);
-}
-
-/**
- * Check out sharp values to evaluate, if waste is in our reach.
- *
- * @ingroup fsmmain
- * @return returns true if waste is in our reach.
- */
-int waste_ahead()
-{
- int sharp[4];
- int cnt = 0, i;
-
- get_short_sharp_mm(sharp);
- printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
- for (i=0; i<4; i++)
- if (sharp[i] > 0 && sharp[i] < 130)
- cnt++;
-
- return ((cnt >= 1) ? 1 : 0);
-}
-
-/**
- * Check out if stack is empty. Use back innerIR sensors to do it.
- *
- * @return returns true if IR sensors indicate the waste
- */
-int stack_empty()
-{
- int status;
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.back & 0xe0;
- ROBOT_UNLOCK(IRsensors);
- return ((status == 0) ? 1 : 0);
-}
-
-/**
- * Evaluation function to be sure the basket is behind us.
- *
- * @ingroup fsmmain
- * @return
- */
-int basket_behind()
-{
- /* FIXME: return true by now, until the back sharps are connected */
- return 1;
-}
-
-/**
- * Try to detect type of the waste.
- *
- * @ingroup fsmmain
- * @return returns type of the waste.
- */
-int recognize_waste()
-{
- int type = UNKNOWN;
- char bit = 0x01;
- char status;
- int i, cnt = 0;
-
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.back & 0x1f;
- ROBOT_UNLOCK(IRsensors);
-
- for (i=0; i<8; i++)
- if (status & (bit << i))
- cnt++;
-
- switch (cnt) {
- case 0:
- type = UNKNOWN;
- break;
- case 1: type = BOTTLE;
- break;
- default:
- type = CAN;
- break;
- }
-
- return type;
-}
-
-/**
- * Evaluation function to see in what direction the waste is.
- *
- * @ingroup fsmmain
- * @return Returns enum value LEFT = 0, CENTER = 1, RIGHT = 2
- */
-
-enum {
- LEFT = 0,
- CENTER,
- RIGHT
-};
-
-int waste_direction() {
- int sharp[4];
- int window[3];
-
- get_short_sharp_mm(sharp);
- window[0] = sharp[0] + sharp[1];
- window[1] = sharp[1] + sharp[2];
- window[2] = sharp[2] + sharp[3];
-
- printf("windows: %d %d %d\n",window[0],window[1],window[2]);
-
- if((window[0] < window[1]) && (window[0] < window[2])) return LEFT;
- if((window[1] <= window[0]) && (window[1] <= window[2])) return CENTER;
- if((window[2] < window[0]) && (window[2] < window[1])) return RIGHT;
- return CENTER;
-}
-
-void get_new_pos(struct est_pos_type *est, struct ref_pos_type *req,
- double l, double phi)
-{
- req->x = est->x + l*cos(est->phi + phi);
- req->y = est->y + l*sin(est->phi + phi);
- req->phi = est->phi + phi;
-}
-
-void robot_turn(struct est_pos_type des_pos)
-{
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- robot_trajectory_new(&tc);
-
- // Add random point to the trajectory.
-// robot_trajectory_add_point(1, 1.5);
- robot_trajectory_add_final_point_notrans(des_pos.x, des_pos.y, TURN(DEG2RAD(90)));
-}
-
-FSM_STATE_DECL(main_init);
-FSM_STATE_DECL(search_waste);
-FSM_STATE_DECL(turn_robot);
-FSM_STATE_DECL(go_for_waste);
-
-FSM_STATE(main_init)
-{
- /* start event ocurred */
- switch (FSM_EVENT) {
- /* FIXME: delete default: for the competititon */
- default:
- case EV_START:
- /* start competition timer */
-// pthread_create(&thid, NULL, wait_for_end, NULL);
- /* init map and shared memory if required */
- ShmapInit(1);
- /* starting position of the robot */
- ROBOT_LOCK(est_pos);
- robot.est_pos.x = 1.0;
- robot.est_pos.y = 1.0;
- robot.est_pos.phi = 0;
-// robot.est_pos.x = 2.98;
-// robot.est_pos.y = 1.88;
-// robot.est_pos.phi = M_PI;
- ROBOT_UNLOCK(est_pos);
- frontDoorDown();
- /* safety zone around playground`s wall */
-// ShmapSetRectangleFlag(0.0, 0.0, 0.1, 2.1, MAP_FLAG_WALL, 0);
-// ShmapSetRectangleFlag(0.0, 0.0, 3.0, 0.1, MAP_FLAG_WALL, 0);
-// ShmapSetRectangleFlag(0.0, 2.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
-// ShmapSetRectangleFlag(2.9, 0.0, 3.0, 2.1, MAP_FLAG_WALL, 0);
- /* cup for batteries */
- ShmapSetRectangleFlag(1.5, 1.4, 1.8, 1.0, MAP_FLAG_WALL, 0);
- /* waste counter */
- ROBOT_LOCK(waste_cnt);
- robot.waste_cnt = 0;
- printf("waste_cnt = %d\n", robot.waste_cnt);
- ROBOT_UNLOCK(waste_cnt);
- FSM_TIMER(2000);
- break;
- case EV_TIMER:
- /* start to do something */
- FSM_TRANSITION(search_waste);
-// FSM_TRANSITION(waste_grab);
- break;
-// default: break;
- }
-}
-
-
-FSM_STATE(search_waste) {
- struct est_pos_type est_pos;
- int dir;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- frontTransporterStop();
- if(waste_ahead()) {
- printf("waste is in front of me\n");
- ROBOT_LOCK(est_pos);
- est_pos = robot.est_pos;
- ROBOT_UNLOCK(est_pos);
-
- dir = waste_direction();
- switch(dir) {
- case LEFT:
-
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(10));
- printf("saw waste on left, actual phi is %f and desired %f\n",
- DEGREES(act_pos.phi), DEGREES(des_pos.phi));
- break;
- case CENTER:
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(0));
- printf("saw waste on center, actual phi is %f and desired %f\n",
- DEGREES(act_pos.phi), DEGREES(des_pos.phi));
- break;
-
- case RIGHT:
- get_new_pos(&act_pos, &des_pos, 0.1, DEG2RAD(-40));
- printf("saw waste on right, actual phi is %f and desired %f\n",
- DEGREES(act_pos.phi), DEGREES(des_pos.phi));
- break;
- default:
- printf("dont know where the waste is, direction returned %d\n",dir);
- FSM_TRANSITION(search_waste);
- break;
- }
- FSM_TRANSITION(go_for_waste);
- }
- else {
- printf("i can't see any waste, sorry\n");
-
-// robot_goto_trans(robot.act_pos.x-0.1, robot.act_pos.y,
-// TURN_CW(robot.act_pos.phi), NULL);
- FSM_TIMER(1000);
- }
- break;
- case EV_TIMER:
- FSM_TRANSITION(search_waste);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(go_for_waste) {
- struct act_pos_type act_pos;
- switch (FSM_EVENT) {
- case EV_ENTRY:
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
- printf("going for waste: \n");
-
- printf("actual position is %f %f %f and final one %f %f %f\n",
- act_pos.x,act_pos.y,DEGREES(act_pos.phi),
- des_pos.x,des_pos.y,DEGREES(des_pos.phi));
- robot_turn(des_pos);
-// robot_goto_trans(des_pos.x, des_pos.y, TURN_CW(des_pos.phi), NULL);
- break;
-
- case EV_MOTION_DONE:
- printf("went for waste and finished\n");
- FSM_TRANSITION(search_waste);
- break;
- default: break;
- }
-}
-
-int main()
-{
- /* robot initialization */
- robot_init();
-
- FSM(MAIN)->debug_states = 1;
-// FSM(MOVE)->debug_states = 1;
-
- robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
- robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
-
- /* Start threads and wait */
- robot_start();
- robot_wait();
-
- /* clean up */
- robot_destroy();
-
- return 0;
-}
+++ /dev/null
-/*
- * testcollavoid.cc 07/10/02
- *
- * Test collision avoidance using pathplan library.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_MAIN
-#include <unistd.h>
-#include <math.h>
-#include <sharp.h>
-#include <robomath.h>
-#include <robot_eb2007.h>
-#include <fsm.h>
-#include <movehelper_eb2007.h>
-#include <trgen.h>
-#include <servos_eb2007.h>
-#include <map.h>
-
-/* fixed points of the trayectory */
-struct act_pos_type trayectory[] = {
- {0.0, 0.0, 0.0},
- {2.98, 1.88, 0.0},
- {2.98, 0, DEG2RAD(270)},
- {0.0, 1.88, 0.0} };
-
-struct TrajectoryConstraints tc = {
- maxv: 0.1, // m/s
- maxomega: 0.5, // rad/s
- maxangacc: 2, // rad/s^2
- maxacc: 0.6, // m/s^2
- maxcenacc: 1, // m/s^2
- maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
-};
-
-#define TRAYECTORY_CNT 4
-static int trind = 0;
-
-/**
- * FSM states start here.
- */
-FSM_STATE_DECL(main_init);
-FSM_STATE_DECL(plan_go);
-
-/**
- * Set starting position, playground`s safety zone and other obstacles.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(main_init)
-{
-
-
-
- FSM_TIMER(100);
- /* start event ocurred */
- switch (FSM_EVENT) {
- default:
- case EV_START:
- case EV_ENTRY:
- /* init map and shared memory if required */
- ShmapInit(1);
- /* starting position of the robot */
- robot_set_act_pos_notrans(trayectory[trind].x, trayectory[trind].y,
- trayectory[trind].phi);
- /* safety zone around playground`s wall */
-// ShmapSetRectangleType(0.0, 0.0, 0.1, 2.1, MAP_WALL_CSPACE);
-// ShmapSetRectangleType(0.0, 0.0, 3.0, 0.1, MAP_WALL_CSPACE);
-// ShmapSetRectangleType(0.0, 2.0, 3.0, 2.1, MAP_WALL_CSPACE);
-// ShmapSetRectangleType(2.9, 0.0, 3.0, 2.1, MAP_WALL_CSPACE);
- /* cup for batteries */
- ShmapSetRectangleFlag(1.5, 1.4, 1.8, 1.0, MAP_FLAG_WALL, 0);
- break;
- case EV_TIMER:
- /* start to do something */
- FSM_TRANSITION(plan_go);
- break;
-// default: break;
- }
-}
-
-/**
- * Go and try to avoid collision.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(plan_go)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- trind++;
- trind = (trind >= TRAYECTORY_CNT) ? 0 : trind;
- robot_goto_trans(trayectory[trind].x, trayectory[trind].y,
- TURN(trayectory[trind].phi), &tc);
- break;
- case EV_MOTION_DONE:
- printf("target achieved! \n");
- FSM_TRANSITION(plan_go);
- break;
- case EV_GOAL_NOT_REACHABLE:
- DBG("Goal is not reachable: %s\n",fsm_event_str(FSM_EVENT));
- break;
- case EV_START:
- break;
- case EV_EXIT:
- //ShmapFree();
- break;
- default: break;
- }
-}
-
-/* main loop */
-int main()
-{
- /* robot initialization */
- robot_init();
-
- FSM(MAIN)->debug_states = 1;
-
- robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
- robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
-// robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
-
- /* Start threads and wait */
- robot_start();
- robot_wait();
-
- /* clean up */
- robot_destroy();
-
- return 0;
-}
+++ /dev/null
-/*
- * testpickup.cc 07/10/01
- *
- * Test pickup mechanism of the robot.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_MAIN
-#include <unistd.h>
-#include <math.h>
-#include <robot_eb2007.h>
-#include <sharp.h>
-#include <robomath.h>
-#include <fsm.h>
-#include <trgen.h>
-#include <movehelper_eb2007.h>
-#include <servos_eb2007.h>
-#include <map.h>
-
-#define STACK_SIZE 3 /* maximal number of bottles before the deposition */
-
-/**
- * Time constants.
- */
-/* time before stopping the deposition belt resp. closing the back door */
-#define DEPOSITE_WAIT 1000
-/* the same as above, but we need to wait a little bit longer for the last one */
-#define LAST_DEPOSITE_WAIT 1500
-/* time before closing the inner door */
-#define WASTE_ROLL_OUT_WAIT 1500
-/* time before another pickup */
-#define NEW_PICK_UP_WAIT 1000
-/* waste detection period */
-#define WASTE_DETECTION_PERIOD 100
-
-enum {
- UNKNOWN = 0,
- BOTTLE,
- CAN
-};
-
-/**
- * Convert sharp`s measured values to mm.
- *
- * @ingroup fsmmain
- */
-void get_short_sharp_mm(int *sharp)
-{
- ROBOT_LOCK(sharpsWaste);
- sharp[0] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short1);
- sharp[1] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short2);
- sharp[2] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short3);
- sharp[3] = s_gp2d120_ir2mmShort(robot.sharpsWaste.short4);
- ROBOT_UNLOCK(sharpsWaste);
-}
-
-/**
- * Check out sharp values to evaluate, if waste is in our reach.
- *
- * @ingroup fsmmain
- * @return returns true if waste is in our reach.
- */
-int waste_ahead()
-{
- int sharp[4];
- int cnt = 0, i;
-
- get_short_sharp_mm(sharp);
- printf("sharp: %d %d %d %d\n", sharp[0], sharp[1], sharp[2], sharp[3]);
- for (i=0; i<4; i++)
- if (sharp[i] > 0 && sharp[i] < 170)
- cnt++;
-
- return ((cnt >= 1) ? 1 : 0);
-}
-
-/**
- * Check out if stack is empty. Use back innerIR sensors to do it.
- *
- * @return returns true if IR sensors indicate the waste
- */
-int stack_empty()
-{
- int status;
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.back & 0xe0;
- ROBOT_UNLOCK(IRsensors);
- return ((status == 0) ? 1 : 0);
-}
-
-/**
- * Evaluation function to be sure the basket is behind us.
- *
- * @ingroup fsmmain
- * @return
- */
-int basket_behind()
-{
- /* FIXME: return true by now, until the back sharps are connected */
- return 1;
-}
-
-/**
- * Try to detect type of the waste using sensors at the front door.
- *
- * @return waste type
- */
-int recognize_waste_phase1()
-{
- char status;
- int front_door_angle;
- int type = UNKNOWN;
- int i, cnt = 0;
- char bit = 0x01;
-
- ROBOT_LOCK(frontDoorAngle);
- front_door_angle = robot.frontDoorAngle.state;
- ROBOT_UNLOCK(frontDoorAngle);
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.front;
- ROBOT_UNLOCK(IRsensors);
-
- if (front_door_angle > 0x220)
- return type;
-
- for (i=0; i<8; i++)
- if (status & (bit << i))
- cnt++;
-
- switch (cnt) {
- case 0:
- type = UNKNOWN;
- break;
- case 1:
- case 2: type = BOTTLE;
- break;
- default:
- type = CAN;
- break;
- }
-
- return type;
-}
-
-/**
- * Try to detect type of the waste using sensors in the back release space.
- *
- * @ingroup fsmmain
- * @return returns type of the waste.
- */
-int recognize_waste_phase2()
-{
- int type = UNKNOWN;
- char bit = 0x01;
- char status;
- int i, cnt = 0;
-
- ROBOT_LOCK(IRsensors);
- status = robot.IRsensors.back & 0x1f;
- ROBOT_UNLOCK(IRsensors);
-
- for (i=0; i<8; i++)
- if (status & (bit << i))
- cnt++;
-
- switch (cnt) {
- case 0:
- type = UNKNOWN;
- break;
- case 1: type = BOTTLE;
- break;
- default:
- type = CAN;
- break;
- }
-
- return type;
-}
-
-/**
- * Try to detect type of the waste.
- *
- * @ingroup fsmmain
- * @return returns type of the waste.
- */
-int recognize_waste()
-{
- int type = UNKNOWN;
-// type = recognize_waste_phase1();
- type = recognize_waste_phase2();
- return type;
-}
-
-/**
- * Deposite bottle to correct basket according to the actual position and
- * the color of our team.
- */
-void deposite_bottle()
-{
- struct act_pos_type act_pos;
- void (*servo)(void);
-
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
-
-#ifdef WE_ARE_BLUE
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = innerTransporterRight;
- else
- servo = backDoorDown;
-#else
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = innerTransporterLeft;
- else
- servo = backDoorDown;
-#endif
- (*servo)();
-}
-
-/**
- * Deposite can to correct basket according to the actual position and
- * the color of our team.
- */
-void deposite_can()
-{
- struct act_pos_type act_pos;
- void (*servo)();
-
- ROBOT_LOCK(act_pos);
- act_pos = robot.act_pos;
- ROBOT_UNLOCK(act_pos);
-
-#ifdef WE_ARE_BLUE
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = backDoorDown;
- else
- servo = innerTransporterLeft;
-#else
- if (fabs(act_pos.phi - DEG2RAD(90)) < DEG2RAD(30))
- servo = backDoorDown;
- else
- servo = innerTransporterRight;
-#endif
- (*servo)();
-}
-
-/**
- * FSM states start here.
- */
-FSM_STATE_DECL(main_init);
-FSM_STATE_DECL(waste_grab);
-FSM_STATE_DECL(waste_deposite);
-FSM_STATE_DECL(waste_deposite_wait);
-
-/**
- * @ingroup fsmmain
- */
-FSM_STATE(main_init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- /* set actual position of the robot */
- robot_set_act_pos_trans(PLAYGROUND_WIDTH_M - ROBOT_WIDTH_M/2,
- 0.0,
- DEG2RAD(180));
- /* waste counter */
- ROBOT_LOCK(waste_cnt);
- robot.waste_cnt = 0;
- printf("waste_cnt = %d\n", robot.waste_cnt);
- ROBOT_UNLOCK(waste_cnt);
- FSM_TIMER(2000);
- break;
- case EV_TIMER:
- frontDoorDown();
- FSM_TRANSITION(waste_grab);
- break;
- default: break;
- }
-}
-
-FSM_STATE(waste_grab) {
- int waste_cnt;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_SIGNAL(PICKUP, EV_PICKUP_START, NULL);
- break;
- case EV_PICKUP_READY:
- FSM_SIGNAL(PICKUP, EV_PICKUP_TRY, NULL);
- break;
- case EV_PICKUP_DONE:
- ROBOT_LOCK(waste_cnt);
- robot.waste_cnt++;
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
- if (waste_cnt == 1 && stack_empty()) {
- printf("fsmmain: pickup done. Youuhouuu, I have "
- "a bottle!! In total: %d\n", waste_cnt);
- } else {
- printf("fsmmain: pickup done. But sensors detect "
- "empty stack!! In total: %d\n", waste_cnt);
- }
- /* go to deposite waste if the stack is full */
- if (waste_cnt >= STACK_SIZE) {
- FSM_TRANSITION(waste_deposite);
- /* continue grabbing waste if there`s space in the stack */
- } else {
- FSM_TIMER(NEW_PICK_UP_WAIT);
- }
- break;
- case EV_PICKUP_JAMMED:
- printf("fsmmain: waste jammed\n");
- FSM_TRANSITION(waste_grab);
- break;
- case EV_PICKUP_FAILED:
- printf("fsmmain: pickup failed, try again\n");
-// FSM_SIGNAL(PICKUP, EV_PICKUP_TRY, NULL);
- FSM_TRANSITION(waste_grab);
- break;
- case EV_TIMER:
- printf("timeout: continue grabbing waste\n");
- FSM_TRANSITION(waste_grab);
- break;
- default:
- break;
- }
-}
-
-/**
- * Deposite waste.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(waste_deposite)
-{
- int waste_cnt, type;
-
- ROBOT_LOCK(waste_cnt);
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
-
- if (stack_empty() && waste_cnt != 0) {
- printf("waste_cnt is %d, but I detect stack empty!!!\n",
- waste_cnt);
- }
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- printf("deposite waste: %d\n", waste_cnt);
- innerDoorDown();
- FSM_TIMER(WASTE_ROLL_OUT_WAIT);
- break;
- case EV_TIMER:
- innerDoorUp();
- type = recognize_waste_phase2();
- printf("waste recognition: ");
- switch (type) {
- case WASTE_CAN:
- printf("it`s a can\n");
- deposite_can();
- break;
- case WASTE_BOTTLE:
- printf("it`s a bottle\n");
- deposite_bottle();
- break;
- default:
- deposite_bottle();
- printf("unidentified\n");
- break;
- }
- /* need to recognize type of waste here */
- FSM_TRANSITION(waste_deposite_wait);
- break;
- default: break;
- }
-}
-
-/**
- * Wait until deposition terminated.
- *
- */
-FSM_STATE(waste_deposite_wait)
-{
- int waste_cnt;
- switch(FSM_EVENT) {
- case EV_ENTRY:
- printf("wait for waste`s deposition\n");
- ROBOT_LOCK(waste_cnt);
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
- if (waste_cnt > 1)
- FSM_TIMER(DEPOSITE_WAIT);
- else
- FSM_TIMER(LAST_DEPOSITE_WAIT);
- break;
- case EV_TIMER:
- ROBOT_LOCK(waste_cnt);
- if (robot.waste_cnt > 0)
- robot.waste_cnt--;
- waste_cnt = robot.waste_cnt;
- ROBOT_UNLOCK(waste_cnt);
- innerTransporterStop();
- backDoorUp();
- if (waste_cnt > 0) {
- FSM_TRANSITION(waste_deposite);
- } else {
- FSM_TRANSITION(waste_grab);
- }
- break;
- default: break;
- }
-}
-
-/* main loop */
-int main()
-{
- /* robot initialization */
- robot_init();
-
-// FSM(MAIN)->debug_states = 1;
- robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
- robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
-// robot.fsm[FSM_ID_LOC].state = &fsm_state_loc_init;
-// robot.fsm[FSM_ID_DET].state = &fsm_state_det_start;
- robot.fsm[FSM_ID_PICKUP].state = &fsm_state_pickup_init;
-
- /* Start threads and wait */
- robot_start();
- robot_wait();
-
- /* clean up */
- robot_destroy();
-
- return 0;
-}
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
#define FSM_DISP
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <signal.h>
#include <pthread.h>
#include <unistd.h>
FSM_TIMER(1000);
break;
case EV_TIMER:
- uoled_send_voltage(&robot.gorte.pwr_voltage);
+ uoled_send_voltage(&robot.orte.pwr_voltage);
uoled_send_state(robot.act_fsm_state_name, strlen(robot.act_fsm_state_name), FSM_STATE_MSG);
uoled_send_state(robot.move_fsm_state_name, strlen(robot.move_fsm_state_name), MOVE_FSM_STATE_MSG);
uoled_set_balls((uint8_t*)&robot.carousel);
- uoled_send_position(&robot.gorte.est_pos);
+ uoled_send_position(&robot.orte.est_pos);
uoled_send_hw_status(robot.hw_status);
ROBOT_LOCK(disp);
if(msg_waiting) {
switch (FSM_EVENT) {
case EV_ENTRY:
- uoled_send_voltage(&robot.gorte.pwr_voltage);
+ uoled_send_voltage(&robot.orte.pwr_voltage);
FSM_TIMER(400);
break;
case EV_TIMER:
switch (FSM_EVENT) {
case EV_ENTRY:
- uoled_send_position(&robot.gorte.est_pos);
+ uoled_send_position(&robot.orte.est_pos);
FSM_TIMER(400);
break;
case EV_TIMER:
#define FSM_JOY
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <fsm.h>
-#include <movehelper_eb2008.h>
-#include <servos_eb2008.h>
+#include <movehelper.h>
+#include <servos.h>
#include <math.h>
#define CLOSE_LIMIT 850
#include <laser-nav.h>
#include <mcl.h>
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <fsm.h>
struct mcl_laser l;
#define FSM_MAIN
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <fsm.h>
#include <unistd.h>
-#include <servos_eb2008.h>
+#include <servos.h>
#include <math.h>
-#include <movehelper_eb2008.h>
+#include <movehelper.h>
#include <map.h>
#include <sharp.h>
#include <robomath.h>
#include "trgen.h"
#include "balet.h"
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <pthread.h>
#include <path_planner.h>
#include <signal.h>
-#include <movehelper_eb2008.h>
+#include <movehelper.h>
#include <sharp.h>
#include <unistd.h>
#include <map.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <fsm.h>
#include <string.h>
#include "robodata.h"
-#include <robot_eb2008.h>
-#include <robodim_eb2008.h>
+#include <robot.h>
+#include <robodim.h>
#include <map.h>
#include <robomath.h>
#include <hokuyo.h>
#ifndef _MAP_HANDLING_H
#define _MAP_HANDLING_H
-#include <robodim_eb2008.h>
+#include <robodim.h>
void * thread_obstacle_forgeting(void * arg);
void update_map(struct sharps_type *s);
#include <trgen.h>
#include <robodata.h>
-#include <robot_eb2008.h>
-#include <movehelper_eb2008.h>
+#include <robot.h>
+#include <movehelper.h>
#include <path_simplifier.h>
#include <stdio.h>
#include <stdlib.h>
// I hope it is not neccessary to lock here
l = (int)(left * mul);
r = (int)(right * mul);
- robot.gorte.motion_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
- robot.gorte.motion_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
+ robot.orte.motion_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
+ robot.orte.motion_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
// DBG("speed l=%8.3g (%6d) r=%8.3g (%6d)",
-// left, l, // robot.gorte.motion_speed.left,
-// right, r //robot.gorte.motion_speed.right
+// left, l, // robot.orte.motion_speed.left,
+// right, r //robot.orte.motion_speed.right
// );
- ORTEPublicationSend(robot.gorte.publication_motion_speed);
+ ORTEPublicationSend(robot.orte.publication_motion_speed);
}
/**
#define MOVEHELPER_EB2008_H
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <trgenconstr.h>
#include <math.h>
*/
#include <map.h>
-#include <movehelper_eb2008.h>
+#include <movehelper.h>
#include <pthread.h>
#include <robomath.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <robot_orte.h>
-#include <servos_eb2008.h>
+#include <servos.h>
#include <signal.h>
#include <sys/time.h>
#include <time.h>
block_signals();
/* initial values */
- robot.gorte.motion_speed.left = 0;
- robot.gorte.motion_speed.right = 0;
+ robot.orte.motion_speed.left = 0;
+ robot.orte.motion_speed.right = 0;
- robot.gorte.pwr_ctrl.voltage33 = 1;
- robot.gorte.pwr_ctrl.voltage50 = 1;
- robot.gorte.pwr_ctrl.voltage80 = 1;
+ robot.orte.pwr_ctrl.voltage33 = 1;
+ robot.orte.pwr_ctrl.voltage50 = 1;
+ robot.orte.pwr_ctrl.voltage80 = 1;
brushes_in();
open_bottom_door();
off_bagr();
off_brush_left();
off_brush_right();
- robot.gorte.motion_speed.right = 0;
- robot.gorte.motion_speed.left = 0;
- ORTEPublicationSend(robot.gorte.publication_motion_speed);
+ robot.orte.motion_speed.right = 0;
+ robot.orte.motion_speed.left = 0;
+ ORTEPublicationSend(robot.orte.publication_motion_speed);
- eb2008_roboorte_destroy(&robot.orte);
- generic_roboorte_destroy(&robot.gorte);
+ robottype_roboorte_destroy(&robot.orte);
return 0;
}
#include <mcl_laser.h>
#include <trgenconstr.h>
#include <robottype.h>
-#include <robottype_eb2008.h>
-#include <roboorte_generic.h>
-#include <roboorte_eb2008.h>
-#include <robodim_eb2008.h>
-#include <roboevent_eb2008.h>
+#include <robottype.h>
+#include <roboorte_robottype.h>
+#include <roboorte.h>
+#include <robodim.h>
+#include <roboevent.h>
#include <fsm.h>
-#include <servos_eb2008.h>
+#include <servos.h>
#include <robot_config.h>
#define FSM_STATE_NAME_MAX_LEN 20
struct drives_type drives;
/* orte */
- struct eb2008_orte_data orte;
- struct generic_orte_data gorte;
+ struct robottype_orte_data orte;
/* sensors */
struct sharps_type sharps;
*/
#include <orte.h>
-#include <roboorte_generic.h>
-#include <roboorte_eb2008.h>
+#include <roboorte_robottype.h>
+#include <roboorte.h>
#include <robodata.h>
-#include <robot_eb2008.h>
-#include <movehelper_eb2008.h>
+#include <robot.h>
+#include <movehelper.h>
#include <math.h>
#include <robomath.h>
#include <laser-nav.h>
int rv = 0;
robot.orte.strength = 20;
- robot.gorte.strength = 20;
- rv = eb2008_roboorte_init(&robot.orte);
- if (rv)
- return rv;
-
- rv = generic_roboorte_init(&robot.gorte);
+ rv = robottype_roboorte_init(&robot.orte);
if (rv)
return rv;
/* creating publishers */
- generic_publisher_motion_irc_create(&robot.gorte, NULL, &robot.gorte);
- generic_publisher_motion_speed_create(&robot.gorte, NULL, &robot.gorte);
- generic_publisher_motion_status_create(&robot.gorte, NULL, &robot.gorte);
- generic_publisher_ref_pos_create(&robot.gorte, send_ref_pos_cb, &robot.gorte);
- generic_publisher_est_pos_create(&robot.gorte, send_est_pos_cb, &robot.gorte);
- generic_publisher_joy_data_create(&robot.gorte, NULL, &robot.gorte);
- generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
- generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);
-
- eb2008_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
- eb2008_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
- eb2008_publisher_laser_cmd_create(&robot.orte, NULL, NULL);
+ robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
+ robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
+ robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
+ robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
+ robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
+ robottype_publisher_joy_data_create(&robot.orte, NULL, &robot.orte);
+ robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
+ robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
+
+ robottype_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_laser_cmd_create(&robot.orte, NULL, NULL);
/* create generic subscribers */
- generic_subscriber_motion_irc_create(&robot.gorte, rcv_motion_irc_cb, &robot.gorte);
- generic_subscriber_motion_speed_create(&robot.gorte, rcv_motion_speed_cb, &robot.gorte);
- generic_subscriber_motion_status_create(&robot.gorte, rcv_motion_status_cb, &robot.gorte);
- generic_subscriber_joy_data_create(&robot.gorte, rcv_joy_data_cb, &robot.gorte);
- generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
- generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);
- generic_subscriber_robot_cmd_create(&robot.gorte, rcv_robot_cmd_cb, &robot.gorte);
- generic_subscriber_hokuyo_scan_create(&robot.gorte, rcv_hokuyo_scan_cb, &robot.gorte);
+ robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
+ robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
+ robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
+ robottype_subscriber_joy_data_create(&robot.orte, rcv_joy_data_cb, &robot.orte);
+ robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
+ robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
+ robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
+ robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
/* create eb2008 subscribers */
- eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
- eb2008_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
- eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
- eb2008_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
- eb2008_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
- eb2008_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
+ robottype_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
+ robottype_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
+ robottype_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
+ robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
+ robottype_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
+ robottype_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);
return rv;
}
* License: GNU GPL v.2
*/
-#include <robot_eb2008.h>
-#include <servos_eb2008.h>
+#include <robot.h>
+#include <servos.h>
/* SERVOS */
void open_top_door()
sharpcalib_SOURCES = sharpcalib.cc
# Libraries linked to all programs in this Makefile
-lib_LOADLIBES = robot_eb2008 mcl robodim_eb2008 laser-nav robomath uoled oledlib sercom roboorte_generic roboorte_eb2008 \
- robottype robottype_eb2008 pthread \
- rt m orte pathplan sharp map fsm rbtree motion
+lib_LOADLIBES = robot mcl robodim laser-nav robomath uoled oledlib \
+ sercom roboorte robottype pthread rt m orte pathplan \
+ sharp map fsm rbtree motion
*/
#define FSM_MAIN
-#include <robot_eb2008.h>
-#include <movehelper_eb2008.h>
+#include <robot.h>
+#include <movehelper.h>
#include <trgen.h>
#include <robomath.h>
#include <uoled.h>
*/
#define FSM_MAIN
-#include <movehelper_eb2008.h>
-#include <robot_eb2008.h>
+#include <movehelper.h>
+#include <robot.h>
#include <trgen.h>
#include <path_planner.h>
#include <stdio.h>
#define FSM_MAIN
#include <robodata.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <fsm.h>
#include <unistd.h>
-#include <servos_eb2008.h>
+#include <servos.h>
#include <math.h>
#include "trgen.h"
-#include <movehelper_eb2008.h>
+#include <movehelper.h>
#include <map.h>
#include <sharp.h>
#include <robomath.h>
*/
#define FSM_MAIN
-#include <robot_eb2008.h>
-#include <movehelper_eb2008.h>
+#include <robot.h>
+#include <movehelper.h>
#include <trgen.h>
#include <robomath.h>
#include <string.h>
*/
#define FSM_MAIN
-#include <movehelper_eb2008.h>
-#include <robot_eb2008.h>
+#include <movehelper.h>
+#include <robot.h>
#include <trgen.h>
#include <robomath.h>
#include <string.h>
*/
#define FSM_MAIN
-#include <movehelper_eb2008.h>
-#include <robot_eb2008.h>
+#include <movehelper.h>
+#include <robot.h>
#include <trgen.h>
#include <robomath.h>
#include <string.h>
*/
#define FSM_MAIN
-#include <movehelper_eb2008.h>
-#include <robot_eb2008.h>
+#include <movehelper.h>
+#include <robot.h>
#include <trgen.h>
#include <robomath.h>
#include <string.h>
#define FSM_MAIN
-#include <robot_eb2008.h>
-#include <movehelper_eb2008.h>
+#include <robot.h>
+#include <movehelper.h>
#include <trgen.h>
#include <robomath.h>
joystick_SOURCES = joystick.cc
# Libraries linked to all programs in this Makefile
-lib_LOADLIBES = robot_eb2008 mcl robodim_eb2008 laser-nav robomath roboorte_eb2008 roboorte_generic \
- robottype robottype_eb2008 pthread rt m orte pathplan sharp map fsm rbtree motion uoled oledlib sercom
+lib_LOADLIBES = robot mcl robodim laser-nav robomath roboorte roboorte \
+ robottype robottype pthread rt m orte pathplan sharp map fsm rbtree motion uoled oledlib sercom
#include <math.h>
#include <robomath.h>
#include <sharp.h>
-#include <robot_eb2008.h>
+#include <robot.h>
#include <fsm.h>
-#include <movehelper_eb2008.h>
-#include <servos_eb2008.h>
+#include <movehelper.h>
+#include <servos.h>
#include <trgen.h>
#include <map.h>
#include <robottype.h>
#include <robottype_eb2007.h>
-#include <robottype_eb2008.h>
+#include <robottype.h>
#include <trgen.h>
#include "playgroundscene.h"
#include <QtGui>
#include "MainWindow.h"
-#include "RobomonRecycling.h"
-#include "RobomonExplorer.h"
#include "RobomonTuning.h"
#include "RobomonAtlantis.h"
#include "SimMcl.h"
event->accept();
}
-void MainWindow::addRobomonRecyclingTab()
-{
- RobomonRecycling *robomonRecycling = new RobomonRecycling();
- tabWidget->addTab(robomonRecycling, tr("Robomon Recycling"));
- tabWidget->setCurrentWidget(robomonRecycling);
-}
-
-void MainWindow::addRobomonExplorerTab()
-{
- RobomonExplorer *robomonExplorer = new RobomonExplorer();
- tabWidget->addTab(robomonExplorer, tr("Robomon Explorer"));
- tabWidget->setCurrentWidget(robomonExplorer);
-}
-
void MainWindow::addRobomonTuningTab()
{
RobomonTuning *robomonTuning = new RobomonTuning();
exitAct->setStatusTip(tr("Exit the application"));
connect(exitAct, SIGNAL(triggered()), this, SLOT(close()));
- robomonRecyclingAct = new QAction(QIcon(":/images/robomon_recycling.png"),
- tr("Robot &Recycling"), this);
- robomonRecyclingAct->setShortcut(tr("Ctrl+R"));
- robomonRecyclingAct->setStatusTip(tr("Robomon of the Robot Recycling"));
- connect(robomonRecyclingAct, SIGNAL(triggered()),
- this, SLOT(addRobomonRecyclingTab()));
-
- robomonExplorerAct = new QAction(QIcon(":/images/robomon_explorer.png"),
- tr("Robot &Explorer"), this);
- robomonExplorerAct->setShortcut(tr("Ctrl+E"));
- robomonExplorerAct->setStatusTip(tr("Robomon of the Robot Explorer"));
- connect(robomonExplorerAct, SIGNAL(triggered()),
- this, SLOT(addRobomonExplorerTab()));
-
robomonTuningAct = new QAction(QIcon(":/images/tuning.png"),
tr("Robot &Tuning"), this);
robomonTuningAct->setShortcut(tr("Ctrl+T"));
fileMenu->addAction(exitAct);
toolsMenu = menuBar()->addMenu(tr("&Tools"));
- toolsMenu->addAction(robomonRecyclingAct);
- toolsMenu->addAction(robomonExplorerAct);
toolsMenu->addAction(robomonTuningAct);
toolsMenu->addAction(robomonAtlantisAct);
toolsMenu->addAction(mclAct);
fileToolBar = addToolBar(tr("File"));
toolsToolBar = addToolBar(tr("Tools"));
- toolsToolBar->addAction(robomonRecyclingAct);
- toolsToolBar->addAction(robomonExplorerAct);
toolsToolBar->addAction(robomonTuningAct);
toolsToolBar->addAction(robomonAtlantisAct);
toolsToolBar->addAction(mclAct);
private slots:
void help();
void about();
- void addRobomonRecyclingTab();
- void addRobomonExplorerTab();
void addRobomonTuningTab();
void addRobomonAtlantisTab();
void addMclTab();
/* actions */
QAction *exitAct;
- QAction *robomonRecyclingAct;
- QAction *robomonExplorerAct;
QAction *robomonTuningAct;
QAction *robomonAtlantisAct;
QAction *mclAct;
#include <orte.h>
#include <path_planner.h>
-#include <robodim_eb2008.h>
+#include <robodim.h>
#include <sharp.h>
#include <trgen.h>
#include <map.h>
#include <QKeyEvent>
#include <QDebug>
#include <QMessageBox>
-#include <servos_eb2008.h>
+#include <servos.h>
RobomonAtlantis::RobomonAtlantis(QWidget *parent)
: QWidget(parent)
void RobomonAtlantis::setVoltage33(int state)
{
if (state)
- orte_generic.pwr_ctrl.voltage33 = true;
+ orte.pwr_ctrl.voltage33 = true;
else
- orte_generic.pwr_ctrl.voltage33 = false;
+ orte.pwr_ctrl.voltage33 = false;
}
void RobomonAtlantis::setVoltage50(int state)
{
if (state)
- orte_generic.pwr_ctrl.voltage50 = true;
+ orte.pwr_ctrl.voltage50 = true;
else
- orte_generic.pwr_ctrl.voltage50 = false;
+ orte.pwr_ctrl.voltage50 = false;
}
void RobomonAtlantis::setVoltage80(int state)
{
if (state)
- orte_generic.pwr_ctrl.voltage80 = true;
+ orte.pwr_ctrl.voltage80 = true;
else
- orte_generic.pwr_ctrl.voltage80 = false;
+ orte.pwr_ctrl.voltage80 = false;
}
void RobomonAtlantis::setLeftMotor(int value)
leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
- orte_generic.motion_speed.left = leftMotor;
- orte_generic.motion_speed.right = rightMotor;
+ orte.motion_speed.left = leftMotor;
+ orte.motion_speed.right = rightMotor;
}
leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
- orte_generic.motion_speed.left = leftMotor;
- orte_generic.motion_speed.right = rightMotor;
+ orte.motion_speed.left = leftMotor;
+ orte.motion_speed.right = rightMotor;
}
void RobomonAtlantis::moveFrontDoor(int state)
{
if (state)
- orte_eb2008.servos.door_bottom = BOTTOM_DOOR_OPEN;
+ orte.servos.door_bottom = BOTTOM_DOOR_OPEN;
else
- orte_eb2008.servos.door_bottom = BOTTOM_DOOR_CLOSE;
+ orte.servos.door_bottom = BOTTOM_DOOR_CLOSE;
}
void RobomonAtlantis::moveTopDoor(int state)
{
if (state)
- orte_eb2008.servos.door_top = TOP_DOOR_OPEN;
+ orte.servos.door_top = TOP_DOOR_OPEN;
else
- orte_eb2008.servos.door_top = TOP_DOOR_CLOSE;
+ orte.servos.door_top = TOP_DOOR_CLOSE;
}
void RobomonAtlantis::moveBackDoor(int state)
{
if (state)
- orte_eb2008.servos.door_back = BACK_DOOR_OPEN;
+ orte.servos.door_back = BACK_DOOR_OPEN;
else
- orte_eb2008.servos.door_back = BACK_DOOR_CLOSE;
+ orte.servos.door_back = BACK_DOOR_CLOSE;
}
void RobomonAtlantis::moveLeftBrush(int state)
{
if (state)
- orte_eb2008.servos.brush_left = BRUSH_LEFT_OPEN;
+ orte.servos.brush_left = BRUSH_LEFT_OPEN;
else
- orte_eb2008.servos.brush_left = BRUSH_LEFT_CLOSE;
+ orte.servos.brush_left = BRUSH_LEFT_CLOSE;
}
void RobomonAtlantis::moveRightBrush(int state)
{
if (state)
- orte_eb2008.servos.brush_right = BRUSH_RIGHT_OPEN;
+ orte.servos.brush_right = BRUSH_RIGHT_OPEN;
else
- orte_eb2008.servos.brush_right = BRUSH_RIGHT_CLOSE;
+ orte.servos.brush_right = BRUSH_RIGHT_CLOSE;
}
void RobomonAtlantis::setDrives(int state)
{
- orte_eb2008.drives.brush_left = leftBrushDial->value();
- orte_eb2008.drives.brush_right = rightBrushDial->value();
- orte_eb2008.drives.bagr = bagrDial->value();
- orte_eb2008.drives.carousel_pos = carouselDial->value();
+ orte.drives.brush_left = leftBrushDial->value();
+ orte.drives.brush_right = rightBrushDial->value();
+ orte.drives.bagr = bagrDial->value();
+ orte.drives.carousel_pos = carouselDial->value();
}
void RobomonAtlantis::setLeftBrushDriveCB(int state) {
if(state)
- orte_eb2008.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
+ orte.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
else
- orte_eb2008.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
+ orte.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
}
void RobomonAtlantis::setRightBrushDriveCB(int state) {
if(state)
- orte_eb2008.drives.brush_right = RIGHT_BRUSH_DRIVE_ON;
+ orte.drives.brush_right = RIGHT_BRUSH_DRIVE_ON;
else
- orte_eb2008.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
+ orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
}
void RobomonAtlantis::setBagrDriveCB(int state) {
if(state)
- orte_eb2008.drives.bagr = BAGR_DRIVE_ON;
+ orte.drives.bagr = BAGR_DRIVE_ON;
else
- orte_eb2008.drives.bagr = BAGR_DRIVE_OFF;
+ orte.drives.bagr = BAGR_DRIVE_OFF;
}
void RobomonAtlantis::setDO(int state)
void RobomonAtlantis::setLaser(int state) {
if(state)
- orte_eb2008.laser_cmd.speed = LASER_DRIVE_ON;
+ orte.laser_cmd.speed = LASER_DRIVE_ON;
else
- orte_eb2008.laser_cmd.speed = LASER_DRIVE_OFF;
- ORTEPublicationSend(orte_eb2008.publication_laser_cmd);
+ orte.laser_cmd.speed = LASER_DRIVE_OFF;
+ ORTEPublicationSend(orte.publication_laser_cmd);
}
void RobomonAtlantis::showMap()
void RobomonAtlantis::setSimulation(int state)
{
if(state) {
- eb2008_publisher_sharps_create(&orte_eb2008,
+ robottype_publisher_sharps_create(&orte,
dummy_publisher_callback, this);
} else {
if (!simulationEnabled)
return;
- eb2008_publisher_sharps_destroy(&orte_eb2008);
+ robottype_publisher_sharps_destroy(&orte);
}
simulationEnabled = state;
}
if (obstacleSimulationTimer)
delete obstacleSimulationTimer;
double distance = 0.8;
- orte_eb2008.sharps.left = distance;
- orte_eb2008.sharps.front_left = distance;
- orte_eb2008.sharps.front_right = distance;
- orte_eb2008.sharps.right = distance;
- ORTEPublicationSend(orte_eb2008.publication_sharps);
+ orte.sharps.left = distance;
+ orte.sharps.front_left = distance;
+ orte.sharps.front_right = distance;
+ orte.sharps.right = distance;
+ ORTEPublicationSend(orte.publication_sharps);
}
}
double distance, wall_distance;
int i;
double *sharp[4] = {
- &orte_eb2008.sharps.left,
- &orte_eb2008.sharps.right,
- &orte_eb2008.sharps.front_left,
- &orte_eb2008.sharps.front_right
+ &orte.sharps.left,
+ &orte.sharps.right,
+ &orte.sharps.front_left,
+ &orte.sharps.front_right
};
distance = wall_distance;
*sharp[i] = distance;
}
- ORTEPublicationSend(orte_eb2008.publication_sharps);
+ ORTEPublicationSend(orte.publication_sharps);
}
void RobomonAtlantis::closeEvent(QCloseEvent *event)
{
- generic_roboorte_destroy(&orte_generic);
- eb2008_roboorte_destroy(&orte_eb2008);
+ robottype_roboorte_destroy(&orte);
}
/**********************************************************************
{
int rv;
- orte_generic.strength = 11;
- orte_eb2008.strength = 11;
+ orte.strength = 11;
- rv = generic_roboorte_init(&orte_generic);
- if (rv) {
- printf("RobomonAtlantis: Unable to initialize ORTE\n");
- }
- rv = eb2008_roboorte_init(&orte_eb2008);
+ rv = robottype_roboorte_init(&orte);
if (rv) {
printf("RobomonAtlantis: Unable to initialize ORTE\n");
}
/* publishers */
- generic_publisher_motion_speed_create(&orte_generic, dummy_publisher_callback, NULL);
+ robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
- generic_publisher_pwr_ctrl_create(&orte_generic, dummy_publisher_callback, NULL);
+ robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
- eb2008_publisher_servos_create(&orte_eb2008, dummy_publisher_callback, NULL);
- eb2008_publisher_drives_create(&orte_eb2008, dummy_publisher_callback, NULL);
- eb2008_publisher_laser_cmd_create(&orte_eb2008, NULL, NULL);
+ robottype_publisher_servos_create(&orte, dummy_publisher_callback, NULL);
+ robottype_publisher_drives_create(&orte, dummy_publisher_callback, NULL);
+ robottype_publisher_laser_cmd_create(&orte, NULL, NULL);
/* subscribers */
- generic_subscriber_pwr_voltage_create(&orte_generic,
+ robottype_subscriber_pwr_voltage_create(&orte,
receivePowerVoltageCallBack, this);
- generic_subscriber_motion_status_create(&orte_generic,
+ robottype_subscriber_motion_status_create(&orte,
receiveMotionStatusCallBack, this);
- generic_subscriber_ref_pos_create(&orte_generic,
+ robottype_subscriber_ref_pos_create(&orte,
receiveActualPositionCallBack, this);
- generic_subscriber_est_pos_create(&orte_generic,
+ robottype_subscriber_est_pos_create(&orte,
receiveEstimatedPositionCallBack, this);
- eb2008_subscriber_sharps_create(&orte_eb2008,
+ robottype_subscriber_sharps_create(&orte,
receiveSharpsCallBack, this);
/*createDISubscriber(this, &orteData);*/
/*createAccelerometerSubscriber(this, &orteData);*/
/*createAccumulatorSubscriber(this, &orteData);*/
/* motors */
- orte_generic.motion_speed.left = 0;
- orte_generic.motion_speed.right = 0;
+ orte.motion_speed.left = 0;
+ orte.motion_speed.right = 0;
/* power management */
- orte_generic.pwr_ctrl.voltage33 = true;
- orte_generic.pwr_ctrl.voltage50 = true;
- orte_generic.pwr_ctrl.voltage80 = true;
+ orte.pwr_ctrl.voltage33 = true;
+ orte.pwr_ctrl.voltage50 = true;
+ orte.pwr_ctrl.voltage80 = true;
voltage33CheckBox->setChecked(true);
voltage50CheckBox->setChecked(true);
voltage80CheckBox->setChecked(true);
/* servos */
- orte_eb2008.servos.door_bottom = FRONT_DOOR_UP; //FIXME
- orte_eb2008.servos.door_back = BACK_DOOR_UP; //FIXME
- orte_eb2008.servos.door_top = TOP_DOOR_UP; //FIXME
+ orte.servos.door_bottom = FRONT_DOOR_UP; //FIXME
+ orte.servos.door_back = BACK_DOOR_UP; //FIXME
+ orte.servos.door_top = TOP_DOOR_UP; //FIXME
bottomDoorCheckBox->setChecked(true);
backDoorCheckBox->setChecked(true);
topDoorCheckBox->setChecked(true);
void RobomonAtlantis::actualPositionReceived()
{
- actPosX->setText(QString("%1").arg(orte_generic.ref_pos.x, 0, 'f', 3));
- actPosY->setText(QString("%1").arg(orte_generic.ref_pos.y, 0, 'f', 3));
+ actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
+ actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
actPosPhi->setText(QString("%1(%2)")
- .arg(DEGREES(orte_generic.ref_pos.phi), 0, 'f', 0)
- .arg(orte_generic.ref_pos.phi, 0, 'f', 1));
- robotActPos->moveRobot(orte_generic.ref_pos.x,
- orte_generic.ref_pos.y, orte_generic.ref_pos.phi);
+ .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
+ .arg(orte.ref_pos.phi, 0, 'f', 1));
+ robotActPos->moveRobot(orte.ref_pos.x,
+ orte.ref_pos.y, orte.ref_pos.phi);
}
void RobomonAtlantis::estimatedPositionReceived()
{
- estPosX->setText(QString("%1").arg(orte_generic.est_pos.x, 0, 'f', 3));
- estPosY->setText(QString("%1").arg(orte_generic.est_pos.y, 0, 'f', 3));
+ estPosX->setText(QString("%1").arg(orte.est_pos.x, 0, 'f', 3));
+ estPosY->setText(QString("%1").arg(orte.est_pos.y, 0, 'f', 3));
estPosPhi->setText(QString("%1(%2)")
- .arg(DEGREES(orte_generic.est_pos.phi), 0, 'f', 0)
- .arg(orte_generic.est_pos.phi, 0, 'f', 1));
- robotEstPos->moveRobot(orte_generic.est_pos.x,
- orte_generic.est_pos.y, orte_generic.est_pos.phi);
+ .arg(DEGREES(orte.est_pos.phi), 0, 'f', 0)
+ .arg(orte.est_pos.phi, 0, 'f', 1));
+ robotEstPos->moveRobot(orte.est_pos.x,
+ orte.est_pos.y, orte.est_pos.phi);
}
void RobomonAtlantis::sharpsReceived()
// WDBG("ORTE received: sharps");
- val_long[0] = (int)((double)orte_eb2008.sharps.left * 1000);
- val_long[1] = (int)((double)orte_eb2008.sharps.front_left * 1000);
- val_long[2] = (int)((double)orte_eb2008.sharps.front_right * 1000);
- val_long[3] = (int)((double)orte_eb2008.sharps.right * 1000);
+ val_long[0] = (int)((double)orte.sharps.left * 1000);
+ val_long[1] = (int)((double)orte.sharps.front_left * 1000);
+ val_long[2] = (int)((double)orte.sharps.front_right * 1000);
+ val_long[3] = (int)((double)orte.sharps.right * 1000);
- val_short[0] = (int)((double)orte_eb2008.sharps.back_left * 1000);
- val_short[1] = (int)((double)orte_eb2008.sharps.back_right * 1000);
+ val_short[0] = (int)((double)orte.sharps.back_left * 1000);
+ val_short[1] = (int)((double)orte.sharps.back_right * 1000);
for (int i=0; i<SHARP_LONG_CNT; i++) {
sharpLongsLabel[i]->setText(QString("%1mm").arg(val_long[i]));
void RobomonAtlantis::powerVoltageReceived()
{
voltage33LineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltage33, 0, 'f', 3));
+ orte.pwr_voltage.voltage33, 0, 'f', 3));
voltage50LineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltage50, 0, 'f', 3));
+ orte.pwr_voltage.voltage50, 0, 'f', 3));
voltage80LineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltage80, 0, 'f', 3));
+ orte.pwr_voltage.voltage80, 0, 'f', 3));
voltageBATLineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltageBAT, 0, 'f', 3));
+ orte.pwr_voltage.voltageBAT, 0, 'f', 3));
}
*/
double RobomonAtlantis::distanceToObstacle(int sharpnum, Point obstacle, double obstacleSize)
{
- struct est_pos_type e = orte_generic.est_pos;
+ struct est_pos_type e = orte.est_pos;
double sensor_a;
struct sharp_pos s = sharp[sharpnum];
Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
#include <trgen.h>
#include "PlaygroundScene.h"
#include "Robot.h"
-#include <roboorte_generic.h>
-#include <roboorte_eb2008.h>
+#include <roboorte_robottype.h>
+#include <roboorte.h>
class QHBoxLayout;
class QVBoxLayout;
************************************************************/
void createOrte();
- struct generic_orte_data orte_generic;
- struct eb2008_orte_data orte_eb2008;
+ struct robottype_orte_data orte;
};
#endif /* ROBOMON_ATLANTIS_H */
#include <orte.h>
#include <path_planner.h>
-#include <robodim_eb2008.h>
+#include <robodim.h>
#include <sharp.h>
#include <trgen.h>
#include <map.h>
#include <QKeyEvent>
#include <QDebug>
#include <QMessageBox>
-#include <servos_eb2008.h>
+#include <servos.h>
RobomonExplorer::RobomonExplorer(QWidget *parent)
: QWidget(parent)
void RobomonExplorer::setVoltage33(int state)
{
if (state)
- orte_generic.pwr_ctrl.voltage33 = true;
+ orte.pwr_ctrl.voltage33 = true;
else
- orte_generic.pwr_ctrl.voltage33 = false;
+ orte.pwr_ctrl.voltage33 = false;
}
void RobomonExplorer::setVoltage50(int state)
{
if (state)
- orte_generic.pwr_ctrl.voltage50 = true;
+ orte.pwr_ctrl.voltage50 = true;
else
- orte_generic.pwr_ctrl.voltage50 = false;
+ orte.pwr_ctrl.voltage50 = false;
}
void RobomonExplorer::setVoltage80(int state)
{
if (state)
- orte_generic.pwr_ctrl.voltage80 = true;
+ orte.pwr_ctrl.voltage80 = true;
else
- orte_generic.pwr_ctrl.voltage80 = false;
+ orte.pwr_ctrl.voltage80 = false;
}
void RobomonExplorer::setLeftMotor(int value)
leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
- orte_generic.motion_speed.left = leftMotor;
- orte_generic.motion_speed.right = rightMotor;
+ orte.motion_speed.left = leftMotor;
+ orte.motion_speed.right = rightMotor;
}
leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
- orte_generic.motion_speed.left = leftMotor;
- orte_generic.motion_speed.right = rightMotor;
+ orte.motion_speed.left = leftMotor;
+ orte.motion_speed.right = rightMotor;
}
void RobomonExplorer::moveFrontDoor(int state)
{
if (state)
- orte_eb2008.servos.door_bottom = BOTTOM_DOOR_OPEN;
+ orte.servos.door_bottom = BOTTOM_DOOR_OPEN;
else
- orte_eb2008.servos.door_bottom = BOTTOM_DOOR_CLOSE;
+ orte.servos.door_bottom = BOTTOM_DOOR_CLOSE;
}
void RobomonExplorer::moveTopDoor(int state)
{
if (state)
- orte_eb2008.servos.door_top = TOP_DOOR_OPEN;
+ orte.servos.door_top = TOP_DOOR_OPEN;
else
- orte_eb2008.servos.door_top = TOP_DOOR_CLOSE;
+ orte.servos.door_top = TOP_DOOR_CLOSE;
}
void RobomonExplorer::moveBackDoor(int state)
{
if (state)
- orte_eb2008.servos.door_back = BACK_DOOR_OPEN;
+ orte.servos.door_back = BACK_DOOR_OPEN;
else
- orte_eb2008.servos.door_back = BACK_DOOR_CLOSE;
+ orte.servos.door_back = BACK_DOOR_CLOSE;
}
void RobomonExplorer::moveLeftBrush(int state)
{
if (state)
- orte_eb2008.servos.brush_left = BRUSH_LEFT_OPEN;
+ orte.servos.brush_left = BRUSH_LEFT_OPEN;
else
- orte_eb2008.servos.brush_left = BRUSH_LEFT_CLOSE;
+ orte.servos.brush_left = BRUSH_LEFT_CLOSE;
}
void RobomonExplorer::moveRightBrush(int state)
{
if (state)
- orte_eb2008.servos.brush_right = BRUSH_RIGHT_OPEN;
+ orte.servos.brush_right = BRUSH_RIGHT_OPEN;
else
- orte_eb2008.servos.brush_right = BRUSH_RIGHT_CLOSE;
+ orte.servos.brush_right = BRUSH_RIGHT_CLOSE;
}
void RobomonExplorer::setDrives(int state)
{
- orte_eb2008.drives.brush_left = leftBrushDial->value();
- orte_eb2008.drives.brush_right = rightBrushDial->value();
- orte_eb2008.drives.bagr = bagrDial->value();
- orte_eb2008.drives.carousel_pos = carouselDial->value();
+ orte.drives.brush_left = leftBrushDial->value();
+ orte.drives.brush_right = rightBrushDial->value();
+ orte.drives.bagr = bagrDial->value();
+ orte.drives.carousel_pos = carouselDial->value();
}
void RobomonExplorer::setLeftBrushDriveCB(int state) {
if(state)
- orte_eb2008.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
+ orte.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
else
- orte_eb2008.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
+ orte.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
}
void RobomonExplorer::setRightBrushDriveCB(int state) {
if(state)
- orte_eb2008.drives.brush_right = RIGHT_BRUSH_DRIVE_ON;
+ orte.drives.brush_right = RIGHT_BRUSH_DRIVE_ON;
else
- orte_eb2008.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
+ orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
}
void RobomonExplorer::setBagrDriveCB(int state) {
if(state)
- orte_eb2008.drives.bagr = BAGR_DRIVE_ON;
+ orte.drives.bagr = BAGR_DRIVE_ON;
else
- orte_eb2008.drives.bagr = BAGR_DRIVE_OFF;
+ orte.drives.bagr = BAGR_DRIVE_OFF;
}
void RobomonExplorer::setDO(int state)
void RobomonExplorer::setLaser(int state) {
if(state)
- orte_eb2008.laser_cmd.speed = LASER_DRIVE_ON;
+ orte.laser_cmd.speed = LASER_DRIVE_ON;
else
- orte_eb2008.laser_cmd.speed = LASER_DRIVE_OFF;
- ORTEPublicationSend(orte_eb2008.publication_laser_cmd);
+ orte.laser_cmd.speed = LASER_DRIVE_OFF;
+ ORTEPublicationSend(orte.publication_laser_cmd);
}
void RobomonExplorer::showMap()
void RobomonExplorer::setSimulation(int state)
{
if(state) {
- eb2008_publisher_sharps_create(&orte_eb2008,
+ robottype_publisher_sharps_create(&orte,
dummy_publisher_callback, this);
} else {
if (!simulationEnabled)
return;
- eb2008_publisher_sharps_destroy(&orte_eb2008);
+ robottype_publisher_sharps_destroy(&orte);
}
simulationEnabled = state;
}
if (obstacleSimulationTimer)
delete obstacleSimulationTimer;
double distance = 0.8;
- orte_eb2008.sharps.left = distance;
- orte_eb2008.sharps.front_left = distance;
- orte_eb2008.sharps.front_right = distance;
- orte_eb2008.sharps.right = distance;
- ORTEPublicationSend(orte_eb2008.publication_sharps);
+ orte.sharps.left = distance;
+ orte.sharps.front_left = distance;
+ orte.sharps.front_right = distance;
+ orte.sharps.right = distance;
+ ORTEPublicationSend(orte.publication_sharps);
}
}
double distance, wall_distance;
int i;
double *sharp[4] = {
- &orte_eb2008.sharps.left,
- &orte_eb2008.sharps.right,
- &orte_eb2008.sharps.front_left,
- &orte_eb2008.sharps.front_right
+ &orte.sharps.left,
+ &orte.sharps.right,
+ &orte.sharps.front_left,
+ &orte.sharps.front_right
};
distance = wall_distance;
*sharp[i] = distance;
}
- ORTEPublicationSend(orte_eb2008.publication_sharps);
+ ORTEPublicationSend(orte.publication_sharps);
}
void RobomonExplorer::closeEvent(QCloseEvent *event)
{
- generic_roboorte_destroy(&orte_generic);
- eb2008_roboorte_destroy(&orte_eb2008);
+ robottype_roboorte_destroy(&orte);
+ robottype_roboorte_destroy(&orte);
}
/**********************************************************************
{
int rv;
- orte_generic.strength = 11;
- orte_eb2008.strength = 11;
+ orte.strength = 11;
+ orte.strength = 11;
- rv = generic_roboorte_init(&orte_generic);
- if (rv) {
- printf("RobomonExplorer: Unable to initialize ORTE\n");
- }
- rv = eb2008_roboorte_init(&orte_eb2008);
+ rv = robottype_roboorte_init(&orte);
if (rv) {
printf("RobomonExplorer: Unable to initialize ORTE\n");
}
/* publishers */
- generic_publisher_motion_speed_create(&orte_generic, dummy_publisher_callback, NULL);
+ robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
- generic_publisher_pwr_ctrl_create(&orte_generic, dummy_publisher_callback, NULL);
+ robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
- eb2008_publisher_servos_create(&orte_eb2008, dummy_publisher_callback, NULL);
- eb2008_publisher_drives_create(&orte_eb2008, dummy_publisher_callback, NULL);
- eb2008_publisher_laser_cmd_create(&orte_eb2008, NULL, NULL);
+ robottype_publisher_servos_create(&orte, dummy_publisher_callback, NULL);
+ robottype_publisher_drives_create(&orte, dummy_publisher_callback, NULL);
+ robottype_publisher_laser_cmd_create(&orte, NULL, NULL);
/* subscribers */
- generic_subscriber_pwr_voltage_create(&orte_generic,
+ robottype_subscriber_pwr_voltage_create(&orte,
receivePowerVoltageCallBack, this);
- generic_subscriber_motion_status_create(&orte_generic,
+ robottype_subscriber_motion_status_create(&orte,
receiveMotionStatusCallBack, this);
- generic_subscriber_ref_pos_create(&orte_generic,
+ robottype_subscriber_ref_pos_create(&orte,
receiveActualPositionCallBack, this);
- generic_subscriber_est_pos_create(&orte_generic,
+ robottype_subscriber_est_pos_create(&orte,
receiveEstimatedPositionCallBack, this);
- eb2008_subscriber_sharps_create(&orte_eb2008,
+ robottype_subscriber_sharps_create(&orte,
receiveSharpsCallBack, this);
/*createDISubscriber(this, &orteData);*/
/*createAccelerometerSubscriber(this, &orteData);*/
/*createAccumulatorSubscriber(this, &orteData);*/
/* motors */
- orte_generic.motion_speed.left = 0;
- orte_generic.motion_speed.right = 0;
+ orte.motion_speed.left = 0;
+ orte.motion_speed.right = 0;
/* power management */
- orte_generic.pwr_ctrl.voltage33 = true;
- orte_generic.pwr_ctrl.voltage50 = true;
- orte_generic.pwr_ctrl.voltage80 = true;
+ orte.pwr_ctrl.voltage33 = true;
+ orte.pwr_ctrl.voltage50 = true;
+ orte.pwr_ctrl.voltage80 = true;
voltage33CheckBox->setChecked(true);
voltage50CheckBox->setChecked(true);
voltage80CheckBox->setChecked(true);
/* servos */
- orte_eb2008.servos.door_bottom = FRONT_DOOR_UP; //FIXME
- orte_eb2008.servos.door_back = BACK_DOOR_UP; //FIXME
- orte_eb2008.servos.door_top = TOP_DOOR_UP; //FIXME
+ orte.servos.door_bottom = FRONT_DOOR_UP; //FIXME
+ orte.servos.door_back = BACK_DOOR_UP; //FIXME
+ orte.servos.door_top = TOP_DOOR_UP; //FIXME
bottomDoorCheckBox->setChecked(true);
backDoorCheckBox->setChecked(true);
topDoorCheckBox->setChecked(true);
void RobomonExplorer::actualPositionReceived()
{
- actPosX->setText(QString("%1").arg(orte_generic.ref_pos.x, 0, 'f', 3));
- actPosY->setText(QString("%1").arg(orte_generic.ref_pos.y, 0, 'f', 3));
+ actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
+ actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
actPosPhi->setText(QString("%1(%2)")
- .arg(DEGREES(orte_generic.ref_pos.phi), 0, 'f', 0)
- .arg(orte_generic.ref_pos.phi, 0, 'f', 1));
- robotActPos->moveRobot(orte_generic.ref_pos.x,
- orte_generic.ref_pos.y, orte_generic.ref_pos.phi);
+ .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
+ .arg(orte.ref_pos.phi, 0, 'f', 1));
+ robotActPos->moveRobot(orte.ref_pos.x,
+ orte.ref_pos.y, orte.ref_pos.phi);
}
void RobomonExplorer::estimatedPositionReceived()
{
- estPosX->setText(QString("%1").arg(orte_generic.est_pos.x, 0, 'f', 3));
- estPosY->setText(QString("%1").arg(orte_generic.est_pos.y, 0, 'f', 3));
+ estPosX->setText(QString("%1").arg(orte.est_pos.x, 0, 'f', 3));
+ estPosY->setText(QString("%1").arg(orte.est_pos.y, 0, 'f', 3));
estPosPhi->setText(QString("%1(%2)")
- .arg(DEGREES(orte_generic.est_pos.phi), 0, 'f', 0)
- .arg(orte_generic.est_pos.phi, 0, 'f', 1));
- robotEstPos->moveRobot(orte_generic.est_pos.x,
- orte_generic.est_pos.y, orte_generic.est_pos.phi);
+ .arg(DEGREES(orte.est_pos.phi), 0, 'f', 0)
+ .arg(orte.est_pos.phi, 0, 'f', 1));
+ robotEstPos->moveRobot(orte.est_pos.x,
+ orte.est_pos.y, orte.est_pos.phi);
}
void RobomonExplorer::sharpsReceived()
// WDBG("ORTE received: sharps");
- val_long[0] = (int)((double)orte_eb2008.sharps.left * 1000);
- val_long[1] = (int)((double)orte_eb2008.sharps.front_left * 1000);
- val_long[2] = (int)((double)orte_eb2008.sharps.front_right * 1000);
- val_long[3] = (int)((double)orte_eb2008.sharps.right * 1000);
+ val_long[0] = (int)((double)orte.sharps.left * 1000);
+ val_long[1] = (int)((double)orte.sharps.front_left * 1000);
+ val_long[2] = (int)((double)orte.sharps.front_right * 1000);
+ val_long[3] = (int)((double)orte.sharps.right * 1000);
- val_short[0] = (int)((double)orte_eb2008.sharps.back_left * 1000);
- val_short[1] = (int)((double)orte_eb2008.sharps.back_right * 1000);
+ val_short[0] = (int)((double)orte.sharps.back_left * 1000);
+ val_short[1] = (int)((double)orte.sharps.back_right * 1000);
for (int i=0; i<SHARP_LONG_CNT; i++) {
sharpLongsLabel[i]->setText(QString("%1mm").arg(val_long[i]));
void RobomonExplorer::powerVoltageReceived()
{
voltage33LineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltage33, 0, 'f', 3));
+ orte.pwr_voltage.voltage33, 0, 'f', 3));
voltage50LineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltage50, 0, 'f', 3));
+ orte.pwr_voltage.voltage50, 0, 'f', 3));
voltage80LineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltage80, 0, 'f', 3));
+ orte.pwr_voltage.voltage80, 0, 'f', 3));
voltageBATLineEdit->setText(QString("%1").arg(
- orte_generic.pwr_voltage.voltageBAT, 0, 'f', 3));
+ orte.pwr_voltage.voltageBAT, 0, 'f', 3));
}
*/
double RobomonExplorer::distanceToObstacle(int sharpnum, Point obstacle, double obstacleSize)
{
- struct est_pos_type e = orte_generic.est_pos;
+ struct est_pos_type e = orte.est_pos;
double sensor_a;
struct sharp_pos s = sharp[sharpnum];
Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
#include <trgen.h>
#include "PlaygroundScene.h"
#include "Robot.h"
-#include <roboorte_generic.h>
-#include <roboorte_eb2008.h>
+#include <roboorte_robottype.h>
+#include <roboorte.h>
class QHBoxLayout;
class QVBoxLayout;
************************************************************/
void createOrte();
- struct generic_orte_data orte_generic;
- struct eb2008_orte_data orte_eb2008;
+ struct robottype_orte_data orte;
};
#endif /* ROBOMON_EXPLORER_H */
#include <trgen.h>
#include "PlaygroundScene.h"
#include "Robot.h"
-#include <roboorte_eb2007.h>
-#include <roboorte_generic.h>
+#include <roboorte.h>
+#include <roboorte_robottype.h>
#include "robomon_orte.h"
class QHBoxLayout;
************************************************************/
void createOrte();
- struct generic_orte_data orte_generic;
- struct eb2007_orte_data orte_eb2007;
+ struct robottype_orte_data orte;
};
#endif /* ROBOMON_RECYCLING_H */
#include <orte.h>
#include <path_planner.h>
-#include <robodim_eb2007.h>
+#include <robodim.h>
#include <sharp.h>
#include <trgen.h>
#include <map.h>
void RobomonTuning::setServos(int value)
{
- orte_eb2008.servos.door_top = (u_int8_t)servoDoorTop->value();
- servoDoorTopLEdit->setText(QString("%1").arg(orte_eb2008.servos.door_top));
- orte_eb2008.servos.door_back = (u_int8_t)servoDoorBack->value();
- servoDoorBackLEdit->setText(QString("%1").arg(orte_eb2008.servos.door_back));
- orte_eb2008.servos.door_bottom = (u_int8_t)servoDoorBottom->value();
- servoDoorBottomLEdit->setText(QString("%1").arg(orte_eb2008.servos.door_bottom));
- orte_eb2008.servos.brush_left = (u_int8_t)servoBrushLeft->value();
- servoBrushLeftLEdit->setText(QString("%1").arg(orte_eb2008.servos.brush_left));
- orte_eb2008.servos.brush_right = (u_int8_t)servoBrushRight->value();
- servoBrushRightLEdit->setText(QString("%1").arg(orte_eb2008.servos.brush_right));
-
- printf("setting servos: %d, %d, %d, %d, %d\n",orte_eb2008.servos.door_top,orte_eb2008.servos.door_back,
- orte_eb2008.servos.door_bottom,orte_eb2008.servos.brush_left,orte_eb2008.servos.brush_right);
-
- ORTEPublicationSend(orte_eb2008.publication_servos);
+ orte.servos.door_top = (u_int8_t)servoDoorTop->value();
+ servoDoorTopLEdit->setText(QString("%1").arg(orte.servos.door_top));
+ orte.servos.door_back = (u_int8_t)servoDoorBack->value();
+ servoDoorBackLEdit->setText(QString("%1").arg(orte.servos.door_back));
+ orte.servos.door_bottom = (u_int8_t)servoDoorBottom->value();
+ servoDoorBottomLEdit->setText(QString("%1").arg(orte.servos.door_bottom));
+ orte.servos.brush_left = (u_int8_t)servoBrushLeft->value();
+ servoBrushLeftLEdit->setText(QString("%1").arg(orte.servos.brush_left));
+ orte.servos.brush_right = (u_int8_t)servoBrushRight->value();
+ servoBrushRightLEdit->setText(QString("%1").arg(orte.servos.brush_right));
+
+ printf("setting servos: %d, %d, %d, %d, %d\n",orte.servos.door_top,orte.servos.door_back,
+ orte.servos.door_bottom,orte.servos.brush_left,orte.servos.brush_right);
+
+ ORTEPublicationSend(orte.publication_servos);
}
void RobomonTuning::setPwrAlert(int value)
{
if(value) {
- orte_generic.pwr_alert.value = 2;
+ orte.pwr_alert.value = 2;
}
else {
- orte_generic.pwr_alert.value = 0;
+ orte.pwr_alert.value = 0;
}
void RobomonTuning::setCanMsg()
{
- orte_generic.can_msg.data1 = canData1->text().toInt();
- orte_generic.can_msg.data2 = canData2->text().toInt();
- orte_generic.can_msg.data3 = canData3->text().toInt();
- orte_generic.can_msg.data4 = canData4->text().toInt();
- orte_generic.can_msg.data5 = canData5->text().toInt();
- orte_generic.can_msg.data6 = canData6->text().toInt();
- orte_generic.can_msg.data7 = canData7->text().toInt();
- orte_generic.can_msg.data8 = canData8->text().toInt();
+ orte.can_msg.data1 = canData1->text().toInt();
+ orte.can_msg.data2 = canData2->text().toInt();
+ orte.can_msg.data3 = canData3->text().toInt();
+ orte.can_msg.data4 = canData4->text().toInt();
+ orte.can_msg.data5 = canData5->text().toInt();
+ orte.can_msg.data6 = canData6->text().toInt();
+ orte.can_msg.data7 = canData7->text().toInt();
+ orte.can_msg.data8 = canData8->text().toInt();
- orte_generic.can_msg.len = canMsgLen->text().toInt();
- orte_generic.can_msg.id = canId->text().toInt();
+ orte.can_msg.len = canMsgLen->text().toInt();
+ orte.can_msg.id = canId->text().toInt();
- ORTEPublicationSend(orte_generic.publication_can_msg);
+ ORTEPublicationSend(orte.publication_can_msg);
}
void RobomonTuning::setMotors(int state)
{
- orte_eb2008.drives.brush_left = brushLeft->value();
- orte_eb2008.drives.brush_right = brushRight->value();
- orte_eb2008.drives.bagr = roboBagr->value();
- orte_eb2008.drives.carousel_pos = carousel->value();
- ORTEPublicationSend(orte_eb2008.publication_drives);
-
- brushLeftLEdit->setText(QString("%1").arg(orte_eb2008.drives.brush_left));
- brushRightLEdit->setText(QString("%1").arg(orte_eb2008.drives.brush_right));
- roboBagrLEdit->setText(QString("%1").arg(orte_eb2008.drives.bagr));
- carouselLEdit->setText(QString("%1").arg(orte_eb2008.drives.carousel_pos));
+ orte.drives.brush_left = brushLeft->value();
+ orte.drives.brush_right = brushRight->value();
+ orte.drives.bagr = roboBagr->value();
+ orte.drives.carousel_pos = carousel->value();
+ ORTEPublicationSend(orte.publication_drives);
+
+ brushLeftLEdit->setText(QString("%1").arg(orte.drives.brush_left));
+ brushRightLEdit->setText(QString("%1").arg(orte.drives.brush_right));
+ roboBagrLEdit->setText(QString("%1").arg(orte.drives.bagr));
+ carouselLEdit->setText(QString("%1").arg(orte.drives.carousel_pos));
}
void RobomonTuning::closeEvent(QCloseEvent *event)
{
- generic_roboorte_destroy(&orte_generic);
- eb2008_roboorte_destroy(&orte_eb2008);
+ robottype_roboorte_destroy(&orte);
}
/**********************************************************************
void RobomonTuning::createOrte()
{
- orte_generic.strength = 12;
- orte_eb2008.strength = 12;
+ orte.strength = 12;
- generic_roboorte_init(&orte_generic);
- eb2008_roboorte_init(&orte_eb2008);
+ robottype_roboorte_init(&orte);
- orte_generic.pwr_alert.value = 0;
+ orte.pwr_alert.value = 0;
/* publishers */
- generic_publisher_can_msg_create(&orte_generic, NULL, NULL);
- generic_publisher_pwr_ctrl_create(&orte_generic, NULL, NULL);
- generic_publisher_pwr_alert_create(&orte_generic, dummy_publisher_callback, NULL);
+ robottype_publisher_can_msg_create(&orte, NULL, NULL);
+ robottype_publisher_pwr_ctrl_create(&orte, NULL, NULL);
+ robottype_publisher_pwr_alert_create(&orte, dummy_publisher_callback, NULL);
- eb2008_publisher_servos_create(&orte_eb2008, NULL, NULL);
- eb2008_publisher_drives_create(&orte_eb2008, NULL, NULL);
+ robottype_publisher_servos_create(&orte, NULL, NULL);
+ robottype_publisher_drives_create(&orte, NULL, NULL);
/*Power supply hack*/
- orte_generic.pwr_ctrl.voltage33 = true;
- orte_generic.pwr_ctrl.voltage50 = true;
- orte_generic.pwr_ctrl.voltage80 = true;
+ orte.pwr_ctrl.voltage33 = true;
+ orte.pwr_ctrl.voltage50 = true;
+ orte.pwr_ctrl.voltage80 = true;
}
/**********************************************************************
#include "PlaygroundScene.h"
#include "Robot.h"
#include "robomon_orte.h"
-#include <roboorte_generic.h>
-#include <roboorte_eb2008.h>
+#include <roboorte_robottype.h>
+#include <roboorte.h>
class QHBoxLayout;
class QVBoxLayout;
************************************************************/
void createOrte();
- struct generic_orte_data orte_generic;
- struct eb2008_orte_data orte_eb2008;
+ struct robottype_orte_data orte;
};
#endif /* ROBOMON_TUNING_H */
#include <math.h>
#include <robomath.h>
-#include <robodim_eb2008.h>
+#include <robodim.h>
#include <mcl.h>
#include <laser-nav.h>
-#include <roboorte_eb2007.h>
+#include <roboorte.h>
#include "PlaygroundScene.h"
#include "MiscGui.h"
void SimMcl::closeEvent(QCloseEvent *event)
{
- generic_roboorte_destroy(&orte_generic);
- eb2008_roboorte_destroy(&orte_eb2008);
+ robottype_roboorte_destroy(&orte);
}
/**********************************************************************
{
int rv;
- orte_generic.strength = 12;
- orte_eb2008.strength = 12;
+ orte.strength = 12;
- rv = generic_roboorte_init(&orte_generic);
+ rv = robottype_roboorte_init(&orte);
if (!rv) {
printf("SimMcl: Unable to initialize ORTE\n");
}
- rv = eb2008_roboorte_init(&orte_eb2008);
- if (rv) {
- printf("SimMcl: Unable to initialize ORTE\n");
- }
-
- generic_subscriber_motion_irc_create(&orte_generic,
+ robottype_subscriber_motion_irc_create(&orte,
rcv_motion_irc_cb, this);
- eb2008_subscriber_laser_data_create(&orte_eb2008,
+ robottype_subscriber_laser_data_create(&orte,
receiveLaserCallBack, this);
/* set actions to do when we receive data from orte */
/*WDBG("ORTE received: laser");*/
measuredAngles.count = 1;
- measuredAngles.val[0] = TIME2ANGLE(orte_eb2008.laser_data.period,
- orte_eb2008.laser_data.measure);
+ measuredAngles.val[0] = TIME2ANGLE(orte.laser_data.period,
+ orte.laser_data.measure);
/*QString dbg = QString("theta=%1")
.arg(RAD2DEG(measuredAngles.val[0]), 0, 'f', 3);
// WDBG("ORTE received: motion_irc");
- curr_irc = orte_generic.motion_irc;
+ curr_irc = orte.motion_irc;
if(firstRun) {
prev_irc = curr_irc;
#include "MclPainter.h"
#include "AnglesHistogramPainter.h"
#include "robomon_orte.h"
-#include <roboorte_eb2008.h>
-#include <roboorte_generic.h>
+#include <roboorte.h>
+#include <roboorte_robottype.h>
#define OPENGL_DEFAULT false
#define ALIASING_DEFAULT true
************************************************************/
void createOrte();
- struct generic_orte_data orte_generic;
- struct eb2008_orte_data orte_eb2008;
+ struct robottype_orte_data orte;
/************************************************************
* PLN - Passive Laser Navigation
#endif
#include <robottype.h>
-#include <robottype_eb2007.h>
-#include <robottype_eb2008.h>
#include <QCoreApplication>
#include "robomon_orte.h"
#include <orte.h>
#include <robottype.h>
-#include <robottype_eb2007.h>
-#include <robottype_eb2008.h>
+#include <robottype.h>
+#include <robottype.h>
/* post event to the receiver, which is a QObject instance */
#define QEVENT(event) (QEvent::Type)(QEvent::User+(event))
QT += opengl
SOURCES += main.cpp \
MainWindow.cpp \
- RobomonRecycling.cpp \
- RobomonExplorer.cpp \
RobomonAtlantis.cpp \
RobomonTuning.cpp \
SimMcl.cpp \
RESOURCES = robomon.qrc
HEADERS += MainWindow.h \
- RobomonRecycling.h \
- RobomonExplorer.h \
RobomonAtlantis.h \
RobomonTuning.h \
SimMcl.h \
MiscGui.h \
robomon_orte.h
-LIBS += -lm -lpthread -lmcl -llaser-nav -lrobodim_eb2008 -lrobomath -lroboorte_generic -lroboorte_eb2008 -lroboorte_eb2007 -lrobottype -lrobottype_eb2007 -lrobottype_eb2008 -lorte -lsharp -lmap -lrobodim_eb2008
+LIBS += -lm -lpthread -lmcl -llaser-nav -lrobodim -lrobomath -lroboorte -lrobottype -lorte -lsharp -lmap
OBJECTS_DIR = ../../../build/linux/_build/user/robomon2/
MOC_DIR = ../../../build/linux/_build/user/robomon2/
# -*- makefile -*-
# Robot's ORTE library
-SUBDIRS = eb2007 eb2008 generic #utils
-
include_HEADERS = roboorte.h
+
+include-pass_HOOKS = roboorte_robottype.c roboorte_robottype.h
+
+roboorte_robottype.c roboorte_robottype.h: $(SOURCES_DIR)/utils/roboortegen.pl $(SOURCES_DIR)/../types/robottype.idl
+ @$(QUIET_CMD_ECHO) " ROBOORTE $@"
+ $(Q)perl -Mlib=$(SOURCES_DIR)/utils/ $(SOURCES_DIR)/utils/roboortegen.pl $(SOURCES_DIR)/../types/robottype.idl
+
+lib_LIBRARIES += roboorte
+roboorte_GEN_SOURCES = roboorte_robottype.c
+include_GEN_HEADERS += roboorte_robottype.h
+
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-# Robot's ORTE library
-
-include-pass_HOOKS = roboorte_eb2007.c roboorte_eb2007.h
-
-roboorte_eb2007.c roboorte_eb2007.h: $(SOURCES_DIR)/../utils/roboortegen.pl $(SOURCES_DIR)/../../types/robottype_eb2007.idl
- @$(QUIET_CMD_ECHO) " ROBOORTE $@"
- $(Q)perl -Mlib=$(SOURCES_DIR)/../utils/ $(SOURCES_DIR)/../utils/roboortegen.pl $(SOURCES_DIR)/../../types/robottype_eb2007.idl eb2007
-
-lib_LIBRARIES += roboorte_eb2007
-roboorte_eb2007_GEN_SOURCES = roboorte_eb2007.c
-include_GEN_HEADERS += roboorte_eb2007.h
-
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-# Robot's ORTE library
-
-include-pass_HOOKS = roboorte_eb2008.c roboorte_eb2008.h
-
-roboorte_eb2008.c roboorte_eb2008.h: $(SOURCES_DIR)/../utils/roboortegen.pl $(SOURCES_DIR)/../../types/robottype_eb2008.idl
- @$(QUIET_CMD_ECHO) " ROBOORTE $@"
- $(Q)perl -Mlib=$(SOURCES_DIR)/../utils/ $(SOURCES_DIR)/../utils/roboortegen.pl $(SOURCES_DIR)/../../types/robottype_eb2008.idl eb2008
-
-lib_LIBRARIES += roboorte_eb2008
-roboorte_eb2008_GEN_SOURCES = roboorte_eb2008.c
-include_GEN_HEADERS += roboorte_eb2008.h
-
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-# Robot's ORTE library
-
-include-pass_HOOKS = roboorte_generic.c roboorte_generic.h
-
-roboorte_generic.c roboorte_generic.h: $(SOURCES_DIR)/../utils/roboortegen.pl $(SOURCES_DIR)/../../types/robottype.idl
- @$(QUIET_CMD_ECHO) " ROBOORTE $@"
- $(Q)perl -Mlib=$(SOURCES_DIR)/../utils/ $(SOURCES_DIR)/../utils/roboortegen.pl $(SOURCES_DIR)/../../types/robottype.idl generic
-
-lib_LIBRARIES += roboorte_generic
-roboorte_generic_GEN_SOURCES = roboorte_generic.c
-include_GEN_HEADERS += roboorte_generic.h
-
include_HEADERS += robottype.h
lib_LIBRARIES = robottype
robottype_SOURCES = robottype.c
-
-include_HEADERS += robottype_eb2007.h
-lib_LIBRARIES += robottype_eb2007
-robottype_eb2007_SOURCES = robottype_eb2007.c
-
-include_HEADERS += robottype_eb2008.h
-lib_LIBRARIES += robottype_eb2008
-robottype_eb2008_SOURCES = robottype_eb2008.c
return ret;
}
+/****************************************************************/
+/* struct - servos */
+/****************************************************************/
+
+void servos_serialize(CDR_Codec *cdrCodec,servos *object) {
+ CORBA_octet_serialize(cdrCodec,&(object->brush_left));
+ CORBA_octet_serialize(cdrCodec,&(object->brush_right));
+ CORBA_octet_serialize(cdrCodec,&(object->door_bottom));
+ CORBA_octet_serialize(cdrCodec,&(object->door_top));
+ CORBA_octet_serialize(cdrCodec,&(object->door_back));
+ CORBA_octet_serialize(cdrCodec,&(object->reserve));
+}
+
+void
+servos_deserialize(CDR_Codec *cdrCodec,servos *object) {
+ CORBA_octet_deserialize(cdrCodec,&(object->brush_left));
+ CORBA_octet_deserialize(cdrCodec,&(object->brush_right));
+ CORBA_octet_deserialize(cdrCodec,&(object->door_bottom));
+ CORBA_octet_deserialize(cdrCodec,&(object->door_top));
+ CORBA_octet_deserialize(cdrCodec,&(object->door_back));
+ CORBA_octet_deserialize(cdrCodec,&(object->reserve));
+}
+
+int
+servos_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+servos_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "servos",
+ (ORTETypeSerialize)servos_serialize,
+ (ORTETypeDeserialize)servos_deserialize,
+ servos_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - drives */
+/****************************************************************/
+
+void drives_serialize(CDR_Codec *cdrCodec,drives *object) {
+ CORBA_octet_serialize(cdrCodec,&(object->brush_left));
+ CORBA_octet_serialize(cdrCodec,&(object->brush_right));
+ CORBA_octet_serialize(cdrCodec,&(object->bagr));
+ CORBA_octet_serialize(cdrCodec,&(object->carousel_pos));
+}
+
+void
+drives_deserialize(CDR_Codec *cdrCodec,drives *object) {
+ CORBA_octet_deserialize(cdrCodec,&(object->brush_left));
+ CORBA_octet_deserialize(cdrCodec,&(object->brush_right));
+ CORBA_octet_deserialize(cdrCodec,&(object->bagr));
+ CORBA_octet_deserialize(cdrCodec,&(object->carousel_pos));
+}
+
+int
+drives_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+drives_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "drives",
+ (ORTETypeSerialize)drives_serialize,
+ (ORTETypeDeserialize)drives_deserialize,
+ drives_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - laser_cmd */
+/****************************************************************/
+
+void laser_cmd_serialize(CDR_Codec *cdrCodec,laser_cmd *object) {
+ CORBA_octet_serialize(cdrCodec,&(object->speed));
+}
+
+void
+laser_cmd_deserialize(CDR_Codec *cdrCodec,laser_cmd *object) {
+ CORBA_octet_deserialize(cdrCodec,&(object->speed));
+}
+
+int
+laser_cmd_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_octet_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+laser_cmd_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "laser_cmd",
+ (ORTETypeSerialize)laser_cmd_serialize,
+ (ORTETypeDeserialize)laser_cmd_deserialize,
+ laser_cmd_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - laser_data */
+/****************************************************************/
+
+void laser_data_serialize(CDR_Codec *cdrCodec,laser_data *object) {
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->period));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measure));
+}
+
+void
+laser_data_deserialize(CDR_Codec *cdrCodec,laser_data *object) {
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->period));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measure));
+}
+
+int
+laser_data_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+laser_data_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "laser_data",
+ (ORTETypeSerialize)laser_data_serialize,
+ (ORTETypeDeserialize)laser_data_deserialize,
+ laser_data_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - laser_meas */
+/****************************************************************/
+
+void laser_meas_serialize(CDR_Codec *cdrCodec,laser_meas *object) {
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->cnt));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->period));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures0));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures1));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures2));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures3));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures4));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures5));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures6));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures7));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures8));
+ CORBA_unsigned_short_serialize(cdrCodec,&(object->measures9));
+}
+
+void
+laser_meas_deserialize(CDR_Codec *cdrCodec,laser_meas *object) {
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->cnt));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->period));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures0));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures1));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures2));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures3));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures4));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures5));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures6));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures7));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures8));
+ CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures9));
+}
+
+int
+laser_meas_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ CORBA_unsigned_short_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+laser_meas_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "laser_meas",
+ (ORTETypeSerialize)laser_meas_serialize,
+ (ORTETypeDeserialize)laser_meas_deserialize,
+ laser_meas_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - sharps */
+/****************************************************************/
+
+void sharps_serialize(CDR_Codec *cdrCodec,sharps *object) {
+ CORBA_double_serialize(cdrCodec,&(object->left));
+ CORBA_double_serialize(cdrCodec,&(object->right));
+ CORBA_double_serialize(cdrCodec,&(object->front_left));
+ CORBA_double_serialize(cdrCodec,&(object->front_right));
+ CORBA_double_serialize(cdrCodec,&(object->back_left));
+ CORBA_double_serialize(cdrCodec,&(object->back_right));
+}
+
+void
+sharps_deserialize(CDR_Codec *cdrCodec,sharps *object) {
+ CORBA_double_deserialize(cdrCodec,&(object->left));
+ CORBA_double_deserialize(cdrCodec,&(object->right));
+ CORBA_double_deserialize(cdrCodec,&(object->front_left));
+ CORBA_double_deserialize(cdrCodec,&(object->front_right));
+ CORBA_double_deserialize(cdrCodec,&(object->back_left));
+ CORBA_double_deserialize(cdrCodec,&(object->back_right));
+}
+
+int
+sharps_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_double_get_max_size(gms);
+ CORBA_double_get_max_size(gms);
+ CORBA_double_get_max_size(gms);
+ CORBA_double_get_max_size(gms);
+ CORBA_double_get_max_size(gms);
+ CORBA_double_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+sharps_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "sharps",
+ (ORTETypeSerialize)sharps_serialize,
+ (ORTETypeDeserialize)sharps_deserialize,
+ sharps_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - cmu */
+/****************************************************************/
+
+void cmu_serialize(CDR_Codec *cdrCodec,cmu *object) {
+ CORBA_octet_serialize(cdrCodec,&(object->color));
+}
+
+void
+cmu_deserialize(CDR_Codec *cdrCodec,cmu *object) {
+ CORBA_octet_deserialize(cdrCodec,&(object->color));
+}
+
+int
+cmu_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_octet_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+cmu_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "cmu",
+ (ORTETypeSerialize)cmu_serialize,
+ (ORTETypeDeserialize)cmu_deserialize,
+ cmu_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - bumper */
+/****************************************************************/
+
+void bumper_serialize(CDR_Codec *cdrCodec,bumper *object) {
+ CORBA_octet_serialize(cdrCodec,&(object->left));
+ CORBA_octet_serialize(cdrCodec,&(object->right));
+}
+
+void
+bumper_deserialize(CDR_Codec *cdrCodec,bumper *object) {
+ CORBA_octet_deserialize(cdrCodec,&(object->left));
+ CORBA_octet_deserialize(cdrCodec,&(object->right));
+}
+
+int
+bumper_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_octet_get_max_size(gms);
+ CORBA_octet_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+bumper_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "bumper",
+ (ORTETypeSerialize)bumper_serialize,
+ (ORTETypeDeserialize)bumper_deserialize,
+ bumper_get_max_size,
+ 0);
+ return ret;
+}
+
CORBA_unsigned_short data34;
};
+#endif
+#if !defined(_servos_defined)
+#define _servos_defined 1
+typedef struct servos_type servos;
+struct servos_type {
+CORBA_octet brush_left;
+CORBA_octet brush_right;
+CORBA_octet door_bottom;
+CORBA_octet door_top;
+CORBA_octet door_back;
+CORBA_octet reserve;
+};
+
+#endif
+#if !defined(_drives_defined)
+#define _drives_defined 1
+typedef struct drives_type drives;
+struct drives_type {
+CORBA_octet brush_left;
+CORBA_octet brush_right;
+CORBA_octet bagr;
+CORBA_octet carousel_pos;
+};
+
+#endif
+#if !defined(_laser_cmd_defined)
+#define _laser_cmd_defined 1
+typedef struct laser_cmd_type laser_cmd;
+struct laser_cmd_type {
+CORBA_octet speed;
+};
+
+#endif
+#if !defined(_laser_data_defined)
+#define _laser_data_defined 1
+typedef struct laser_data_type laser_data;
+struct laser_data_type {
+CORBA_unsigned_short period;
+CORBA_unsigned_short measure;
+};
+
+#endif
+#if !defined(_laser_meas_defined)
+#define _laser_meas_defined 1
+typedef struct laser_meas_type laser_meas;
+struct laser_meas_type {
+CORBA_unsigned_short cnt;
+CORBA_unsigned_short period;
+CORBA_unsigned_short measures0;
+CORBA_unsigned_short measures1;
+CORBA_unsigned_short measures2;
+CORBA_unsigned_short measures3;
+CORBA_unsigned_short measures4;
+CORBA_unsigned_short measures5;
+CORBA_unsigned_short measures6;
+CORBA_unsigned_short measures7;
+CORBA_unsigned_short measures8;
+CORBA_unsigned_short measures9;
+};
+
+#endif
+#if !defined(_sharps_defined)
+#define _sharps_defined 1
+typedef struct sharps_type sharps;
+struct sharps_type {
+CORBA_double left;
+CORBA_double right;
+CORBA_double front_left;
+CORBA_double front_right;
+CORBA_double back_left;
+CORBA_double back_right;
+};
+
+#endif
+#if !defined(_cmu_defined)
+#define _cmu_defined 1
+typedef struct cmu_type cmu;
+struct cmu_type {
+CORBA_octet color;
+};
+
+#endif
+#if !defined(_bumper_defined)
+#define _bumper_defined 1
+typedef struct bumper_type bumper;
+struct bumper_type {
+CORBA_octet left;
+CORBA_octet right;
+};
+
#endif
/** impls declarations **/
int hokuyo_scan_get_max_size(ORTEGetMaxSizeParam *gms);
Boolean hokuyo_scan_type_register(ORTEDomain *d);
+void servos_serialize(CDR_Codec *cdrCodec,servos *object);
+void servos_deserialize(CDR_Codec *cdrCodec,servos *object);
+int servos_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean servos_type_register(ORTEDomain *d);
+
+void drives_serialize(CDR_Codec *cdrCodec,drives *object);
+void drives_deserialize(CDR_Codec *cdrCodec,drives *object);
+int drives_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean drives_type_register(ORTEDomain *d);
+
+void laser_cmd_serialize(CDR_Codec *cdrCodec,laser_cmd *object);
+void laser_cmd_deserialize(CDR_Codec *cdrCodec,laser_cmd *object);
+int laser_cmd_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean laser_cmd_type_register(ORTEDomain *d);
+
+void laser_data_serialize(CDR_Codec *cdrCodec,laser_data *object);
+void laser_data_deserialize(CDR_Codec *cdrCodec,laser_data *object);
+int laser_data_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean laser_data_type_register(ORTEDomain *d);
+
+void laser_meas_serialize(CDR_Codec *cdrCodec,laser_meas *object);
+void laser_meas_deserialize(CDR_Codec *cdrCodec,laser_meas *object);
+int laser_meas_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean laser_meas_type_register(ORTEDomain *d);
+
+void sharps_serialize(CDR_Codec *cdrCodec,sharps *object);
+void sharps_deserialize(CDR_Codec *cdrCodec,sharps *object);
+int sharps_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean sharps_type_register(ORTEDomain *d);
+
+void cmu_serialize(CDR_Codec *cdrCodec,cmu *object);
+void cmu_deserialize(CDR_Codec *cdrCodec,cmu *object);
+int cmu_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean cmu_type_register(ORTEDomain *d);
+
+void bumper_serialize(CDR_Codec *cdrCodec,bumper *object);
+void bumper_deserialize(CDR_Codec *cdrCodec,bumper *object);
+int bumper_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean bumper_type_register(ORTEDomain *d);
+
#ifdef __cplusplus
}
unsigned short data33;
unsigned short data34;
};
+struct servos {
+ octet brush_left;
+ octet brush_right;
+ octet door_bottom;
+ octet door_top;
+ octet door_back;
+ octet reserve;
+};
+
+struct drives {
+ octet brush_left;
+ octet brush_right;
+ octet bagr;
+ octet carousel_pos;
+};
+
+struct laser_cmd {
+ octet speed;
+};
+
+struct laser_data {
+ unsigned short period;
+ unsigned short measure;
+};
+
+struct laser_meas {
+ unsigned short cnt;
+ unsigned short period;
+ unsigned short measures0;
+ unsigned short measures1;
+ unsigned short measures2;
+ unsigned short measures3;
+ unsigned short measures4;
+ unsigned short measures5;
+ unsigned short measures6;
+ unsigned short measures7;
+ unsigned short measures8;
+ unsigned short measures9;
+};
+
+struct sharps {
+ double left;
+ double right;
+ double front_left;
+ double front_right;
+ double back_left;
+ double back_right;
+};
+
+struct cmu {
+ octet color;
+};
+
+struct bumper {
+ octet left;
+ octet right;
+};
+++ /dev/null
-/*
- * This file was generated by orte-idl - DO NOT EDIT!
- */
-
-#include "robottype_eb2007.h"
-
-/****************************************************************/
-/* struct - stateInnerIR */
-/****************************************************************/
-
-void stateInnerIR_serialize(CDR_Codec *cdrCodec,stateInnerIR *object) {
- CORBA_octet_serialize(cdrCodec,&(object->front));
- CORBA_octet_serialize(cdrCodec,&(object->back));
-}
-
-void
-stateInnerIR_deserialize(CDR_Codec *cdrCodec,stateInnerIR *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->front));
- CORBA_octet_deserialize(cdrCodec,&(object->back));
-}
-
-int
-stateInnerIR_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-stateInnerIR_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "stateInnerIR",
- (ORTETypeSerialize)stateInnerIR_serialize,
- (ORTETypeDeserialize)stateInnerIR_deserialize,
- stateInnerIR_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - stateDigIn */
-/****************************************************************/
-
-void stateDigIn_serialize(CDR_Codec *cdrCodec,stateDigIn *object) {
- CORBA_octet_serialize(cdrCodec,&(object->state));
-}
-
-void
-stateDigIn_deserialize(CDR_Codec *cdrCodec,stateDigIn *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->state));
-}
-
-int
-stateDigIn_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-stateDigIn_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "stateDigIn",
- (ORTETypeSerialize)stateDigIn_serialize,
- (ORTETypeDeserialize)stateDigIn_deserialize,
- stateDigIn_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - stateServa */
-/****************************************************************/
-
-void stateServa_serialize(CDR_Codec *cdrCodec,stateServa *object) {
- CORBA_octet_serialize(cdrCodec,&(object->frontDoor));
- CORBA_octet_serialize(cdrCodec,&(object->innerDoor));
- CORBA_octet_serialize(cdrCodec,&(object->backDoor));
- CORBA_octet_serialize(cdrCodec,&(object->topDoor));
- CORBA_octet_serialize(cdrCodec,&(object->bottomDoor));
- CORBA_octet_serialize(cdrCodec,&(object->leftBrush));
- CORBA_octet_serialize(cdrCodec,&(object->rightBrush));
- CORBA_octet_serialize(cdrCodec,&(object->reserve));
- CORBA_octet_serialize(cdrCodec,&(object->release));
- CORBA_octet_serialize(cdrCodec,&(object->transporterFront));
- CORBA_octet_serialize(cdrCodec,&(object->transporterInner));
-}
-
-void
-stateServa_deserialize(CDR_Codec *cdrCodec,stateServa *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->frontDoor));
- CORBA_octet_deserialize(cdrCodec,&(object->innerDoor));
- CORBA_octet_deserialize(cdrCodec,&(object->backDoor));
- CORBA_octet_deserialize(cdrCodec,&(object->topDoor));
- CORBA_octet_deserialize(cdrCodec,&(object->bottomDoor));
- CORBA_octet_deserialize(cdrCodec,&(object->leftBrush));
- CORBA_octet_deserialize(cdrCodec,&(object->rightBrush));
- CORBA_octet_deserialize(cdrCodec,&(object->reserve));
- CORBA_octet_deserialize(cdrCodec,&(object->release));
- CORBA_octet_deserialize(cdrCodec,&(object->transporterFront));
- CORBA_octet_deserialize(cdrCodec,&(object->transporterInner));
-}
-
-int
-stateServa_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-stateServa_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "stateServa",
- (ORTETypeSerialize)stateServa_serialize,
- (ORTETypeDeserialize)stateServa_deserialize,
- stateServa_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - lowerSharps */
-/****************************************************************/
-
-void lowerSharps_serialize(CDR_Codec *cdrCodec,lowerSharps *object) {
- CORBA_short_serialize(cdrCodec,&(object->sharp1));
- CORBA_short_serialize(cdrCodec,&(object->sharp2));
- CORBA_short_serialize(cdrCodec,&(object->sharp3));
- CORBA_short_serialize(cdrCodec,&(object->sharp4));
-}
-
-void
-lowerSharps_deserialize(CDR_Codec *cdrCodec,lowerSharps *object) {
- CORBA_short_deserialize(cdrCodec,&(object->sharp1));
- CORBA_short_deserialize(cdrCodec,&(object->sharp2));
- CORBA_short_deserialize(cdrCodec,&(object->sharp3));
- CORBA_short_deserialize(cdrCodec,&(object->sharp4));
-}
-
-int
-lowerSharps_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_short_get_max_size(gms);
- CORBA_short_get_max_size(gms);
- CORBA_short_get_max_size(gms);
- CORBA_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-lowerSharps_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "lowerSharps",
- (ORTETypeSerialize)lowerSharps_serialize,
- (ORTETypeDeserialize)lowerSharps_deserialize,
- lowerSharps_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - adcs */
-/****************************************************************/
-
-void adcs_serialize(CDR_Codec *cdrCodec,adcs *object) {
- CORBA_octet_serialize(cdrCodec,&(object->sharpLong1));
- CORBA_octet_serialize(cdrCodec,&(object->sharpLong2));
- CORBA_octet_serialize(cdrCodec,&(object->sharpLong3));
- CORBA_octet_serialize(cdrCodec,&(object->sharpShort1));
- CORBA_octet_serialize(cdrCodec,&(object->sharpShort2));
- CORBA_octet_serialize(cdrCodec,&(object->sharpShort3));
- CORBA_octet_serialize(cdrCodec,&(object->sharpShort4));
- CORBA_octet_serialize(cdrCodec,&(object->frontDoor));
-}
-
-void
-adcs_deserialize(CDR_Codec *cdrCodec,adcs *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->sharpLong1));
- CORBA_octet_deserialize(cdrCodec,&(object->sharpLong2));
- CORBA_octet_deserialize(cdrCodec,&(object->sharpLong3));
- CORBA_octet_deserialize(cdrCodec,&(object->sharpShort1));
- CORBA_octet_deserialize(cdrCodec,&(object->sharpShort2));
- CORBA_octet_deserialize(cdrCodec,&(object->sharpShort3));
- CORBA_octet_deserialize(cdrCodec,&(object->sharpShort4));
- CORBA_octet_deserialize(cdrCodec,&(object->frontDoor));
-}
-
-int
-adcs_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-adcs_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "adcs",
- (ORTETypeSerialize)adcs_serialize,
- (ORTETypeDeserialize)adcs_deserialize,
- adcs_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - stateFrontDoor */
-/****************************************************************/
-
-void stateFrontDoor_serialize(CDR_Codec *cdrCodec,stateFrontDoor *object) {
- CORBA_short_serialize(cdrCodec,&(object->state));
-}
-
-void
-stateFrontDoor_deserialize(CDR_Codec *cdrCodec,stateFrontDoor *object) {
- CORBA_short_deserialize(cdrCodec,&(object->state));
-}
-
-int
-stateFrontDoor_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-stateFrontDoor_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "stateFrontDoor",
- (ORTETypeSerialize)stateFrontDoor_serialize,
- (ORTETypeDeserialize)stateFrontDoor_deserialize,
- stateFrontDoor_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - sharpShorts */
-/****************************************************************/
-
-void sharpShorts_serialize(CDR_Codec *cdrCodec,sharpShorts *object) {
- CORBA_short_serialize(cdrCodec,&(object->short1));
- CORBA_short_serialize(cdrCodec,&(object->short2));
- CORBA_short_serialize(cdrCodec,&(object->short3));
- CORBA_short_serialize(cdrCodec,&(object->short4));
-}
-
-void
-sharpShorts_deserialize(CDR_Codec *cdrCodec,sharpShorts *object) {
- CORBA_short_deserialize(cdrCodec,&(object->short1));
- CORBA_short_deserialize(cdrCodec,&(object->short2));
- CORBA_short_deserialize(cdrCodec,&(object->short3));
- CORBA_short_deserialize(cdrCodec,&(object->short4));
-}
-
-int
-sharpShorts_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_short_get_max_size(gms);
- CORBA_short_get_max_size(gms);
- CORBA_short_get_max_size(gms);
- CORBA_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-sharpShorts_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "sharpShorts",
- (ORTETypeSerialize)sharpShorts_serialize,
- (ORTETypeDeserialize)sharpShorts_deserialize,
- sharpShorts_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - sharpLongs */
-/****************************************************************/
-
-void sharpLongs_serialize(CDR_Codec *cdrCodec,sharpLongs *object) {
- CORBA_double_serialize(cdrCodec,&(object->longSharpDist1));
- CORBA_double_serialize(cdrCodec,&(object->longSharpDist2));
- CORBA_double_serialize(cdrCodec,&(object->longSharpDist3));
-}
-
-void
-sharpLongs_deserialize(CDR_Codec *cdrCodec,sharpLongs *object) {
- CORBA_double_deserialize(cdrCodec,&(object->longSharpDist1));
- CORBA_double_deserialize(cdrCodec,&(object->longSharpDist2));
- CORBA_double_deserialize(cdrCodec,&(object->longSharpDist3));
-}
-
-int
-sharpLongs_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-sharpLongs_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "sharpLongs",
- (ORTETypeSerialize)sharpLongs_serialize,
- (ORTETypeDeserialize)sharpLongs_deserialize,
- sharpLongs_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - laserData */
-/****************************************************************/
-
-void laserData_serialize(CDR_Codec *cdrCodec,laserData *object) {
- CORBA_unsigned_short_serialize(cdrCodec,&(object->cnt));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->period));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures0));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures1));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures2));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures3));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures4));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures5));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures6));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures7));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures8));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures9));
-}
-
-void
-laserData_deserialize(CDR_Codec *cdrCodec,laserData *object) {
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->cnt));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->period));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures0));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures1));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures2));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures3));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures4));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures5));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures6));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures7));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures8));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures9));
-}
-
-int
-laserData_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-laserData_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "laserData",
- (ORTETypeSerialize)laserData_serialize,
- (ORTETypeDeserialize)laserData_deserialize,
- laserData_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - accumulator */
-/****************************************************************/
-
-void accumulator_serialize(CDR_Codec *cdrCodec,accumulator *object) {
- CORBA_unsigned_short_serialize(cdrCodec,&(object->position1));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->position2));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->position3));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->position4));
-}
-
-void
-accumulator_deserialize(CDR_Codec *cdrCodec,accumulator *object) {
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->position1));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->position2));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->position3));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->position4));
-}
-
-int
-accumulator_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-accumulator_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "accumulator",
- (ORTETypeSerialize)accumulator_serialize,
- (ORTETypeDeserialize)accumulator_deserialize,
- accumulator_get_max_size,
- 0);
- return ret;
-}
-
+++ /dev/null
-/*
- * This file was generated by orte-idl - DO NOT EDIT!
- */
-
-#ifndef robottype_eb2007_H
-#define robottype_eb2007_H 1
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-#ifndef EXCLUDE_ORTE_H
-#include <orte.h>
-#endif /* EXCLUDE_ORTE_H */
-
-/** typedefs **/
-#if !defined(_stateInnerIR_defined)
-#define _stateInnerIR_defined 1
-typedef struct stateInnerIR_type stateInnerIR;
-struct stateInnerIR_type {
-CORBA_octet front;
-CORBA_octet back;
-};
-
-#endif
-#if !defined(_stateDigIn_defined)
-#define _stateDigIn_defined 1
-typedef struct stateDigIn_type stateDigIn;
-struct stateDigIn_type {
-CORBA_octet state;
-};
-
-#endif
-#if !defined(_stateServa_defined)
-#define _stateServa_defined 1
-typedef struct stateServa_type stateServa;
-struct stateServa_type {
-CORBA_octet frontDoor;
-CORBA_octet innerDoor;
-CORBA_octet backDoor;
-CORBA_octet topDoor;
-CORBA_octet bottomDoor;
-CORBA_octet leftBrush;
-CORBA_octet rightBrush;
-CORBA_octet reserve;
-CORBA_octet release;
-CORBA_octet transporterFront;
-CORBA_octet transporterInner;
-};
-
-#endif
-#if !defined(_lowerSharps_defined)
-#define _lowerSharps_defined 1
-typedef struct lowerSharps_type lowerSharps;
-struct lowerSharps_type {
-CORBA_short sharp1;
-CORBA_short sharp2;
-CORBA_short sharp3;
-CORBA_short sharp4;
-};
-
-#endif
-#if !defined(_adcs_defined)
-#define _adcs_defined 1
-typedef struct adcs_type adcs;
-struct adcs_type {
-CORBA_octet sharpLong1;
-CORBA_octet sharpLong2;
-CORBA_octet sharpLong3;
-CORBA_octet sharpShort1;
-CORBA_octet sharpShort2;
-CORBA_octet sharpShort3;
-CORBA_octet sharpShort4;
-CORBA_octet frontDoor;
-};
-
-#endif
-#if !defined(_stateFrontDoor_defined)
-#define _stateFrontDoor_defined 1
-typedef struct stateFrontDoor_type stateFrontDoor;
-struct stateFrontDoor_type {
-CORBA_short state;
-};
-
-#endif
-#if !defined(_sharpShorts_defined)
-#define _sharpShorts_defined 1
-typedef struct sharpShorts_type sharpShorts;
-struct sharpShorts_type {
-CORBA_short short1;
-CORBA_short short2;
-CORBA_short short3;
-CORBA_short short4;
-};
-
-#endif
-#if !defined(_sharpLongs_defined)
-#define _sharpLongs_defined 1
-typedef struct sharpLongs_type sharpLongs;
-struct sharpLongs_type {
-CORBA_double longSharpDist1;
-CORBA_double longSharpDist2;
-CORBA_double longSharpDist3;
-};
-
-#endif
-#if !defined(_laserData_defined)
-#define _laserData_defined 1
-typedef struct laserData_type laserData;
-struct laserData_type {
-CORBA_unsigned_short cnt;
-CORBA_unsigned_short period;
-CORBA_unsigned_short measures0;
-CORBA_unsigned_short measures1;
-CORBA_unsigned_short measures2;
-CORBA_unsigned_short measures3;
-CORBA_unsigned_short measures4;
-CORBA_unsigned_short measures5;
-CORBA_unsigned_short measures6;
-CORBA_unsigned_short measures7;
-CORBA_unsigned_short measures8;
-CORBA_unsigned_short measures9;
-};
-
-#endif
-#if !defined(_accumulator_defined)
-#define _accumulator_defined 1
-typedef struct accumulator_type accumulator;
-struct accumulator_type {
-CORBA_unsigned_short position1;
-CORBA_unsigned_short position2;
-CORBA_unsigned_short position3;
-CORBA_unsigned_short position4;
-};
-
-#endif
-
-/** impls declarations **/
-void stateInnerIR_serialize(CDR_Codec *cdrCodec,stateInnerIR *object);
-void stateInnerIR_deserialize(CDR_Codec *cdrCodec,stateInnerIR *object);
-int stateInnerIR_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean stateInnerIR_type_register(ORTEDomain *d);
-
-void stateDigIn_serialize(CDR_Codec *cdrCodec,stateDigIn *object);
-void stateDigIn_deserialize(CDR_Codec *cdrCodec,stateDigIn *object);
-int stateDigIn_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean stateDigIn_type_register(ORTEDomain *d);
-
-void stateServa_serialize(CDR_Codec *cdrCodec,stateServa *object);
-void stateServa_deserialize(CDR_Codec *cdrCodec,stateServa *object);
-int stateServa_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean stateServa_type_register(ORTEDomain *d);
-
-void lowerSharps_serialize(CDR_Codec *cdrCodec,lowerSharps *object);
-void lowerSharps_deserialize(CDR_Codec *cdrCodec,lowerSharps *object);
-int lowerSharps_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean lowerSharps_type_register(ORTEDomain *d);
-
-void adcs_serialize(CDR_Codec *cdrCodec,adcs *object);
-void adcs_deserialize(CDR_Codec *cdrCodec,adcs *object);
-int adcs_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean adcs_type_register(ORTEDomain *d);
-
-void stateFrontDoor_serialize(CDR_Codec *cdrCodec,stateFrontDoor *object);
-void stateFrontDoor_deserialize(CDR_Codec *cdrCodec,stateFrontDoor *object);
-int stateFrontDoor_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean stateFrontDoor_type_register(ORTEDomain *d);
-
-void sharpShorts_serialize(CDR_Codec *cdrCodec,sharpShorts *object);
-void sharpShorts_deserialize(CDR_Codec *cdrCodec,sharpShorts *object);
-int sharpShorts_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean sharpShorts_type_register(ORTEDomain *d);
-
-void sharpLongs_serialize(CDR_Codec *cdrCodec,sharpLongs *object);
-void sharpLongs_deserialize(CDR_Codec *cdrCodec,sharpLongs *object);
-int sharpLongs_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean sharpLongs_type_register(ORTEDomain *d);
-
-void laserData_serialize(CDR_Codec *cdrCodec,laserData *object);
-void laserData_deserialize(CDR_Codec *cdrCodec,laserData *object);
-int laserData_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean laserData_type_register(ORTEDomain *d);
-
-void accumulator_serialize(CDR_Codec *cdrCodec,accumulator *object);
-void accumulator_deserialize(CDR_Codec *cdrCodec,accumulator *object);
-int accumulator_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean accumulator_type_register(ORTEDomain *d);
-
-
-#ifdef __cplusplus
-}
-#endif /* __cplusplus */
-
-#endif
+++ /dev/null
-struct stateInnerIR {
- octet front;
- octet back;
-};
-
-struct stateDigIn {
- octet state;
-};
-
-struct stateServa {
- octet frontDoor;
- octet innerDoor;
- octet backDoor;
- octet topDoor;
- octet bottomDoor;
- octet leftBrush;
- octet rightBrush;
- octet reserve;
- octet release;
- octet transporterFront;
- octet transporterInner;
-};
-
-struct lowerSharps {
- short sharp1;
- short sharp2;
- short sharp3;
- short sharp4;
-};
-
-struct adcs {
- octet sharpLong1;
- octet sharpLong2;
- octet sharpLong3;
- octet sharpShort1;
- octet sharpShort2;
- octet sharpShort3;
- octet sharpShort4;
- octet frontDoor;
-};
-
-struct stateFrontDoor {
- short state;
-};
-
-struct sharpShorts {
- short short1;
- short short2;
- short short3;
- short short4;
-};
-
-struct sharpLongs {
- double longSharpDist1;
- double longSharpDist2;
- double longSharpDist3;
-};
-
-struct laserData {
- unsigned short cnt;
- unsigned short period;
- unsigned short measures0;
- unsigned short measures1;
- unsigned short measures2;
- unsigned short measures3;
- unsigned short measures4;
- unsigned short measures5;
- unsigned short measures6;
- unsigned short measures7;
- unsigned short measures8;
- unsigned short measures9;
-};
-
-struct accumulator {
- unsigned short position1;
- unsigned short position2;
- unsigned short position3;
- unsigned short position4;
-};
+++ /dev/null
-/*
- * This file was generated by orte-idl - DO NOT EDIT!
- */
-
-#include "robottype_eb2008.h"
-
-/****************************************************************/
-/* struct - servos */
-/****************************************************************/
-
-void servos_serialize(CDR_Codec *cdrCodec,servos *object) {
- CORBA_octet_serialize(cdrCodec,&(object->brush_left));
- CORBA_octet_serialize(cdrCodec,&(object->brush_right));
- CORBA_octet_serialize(cdrCodec,&(object->door_bottom));
- CORBA_octet_serialize(cdrCodec,&(object->door_top));
- CORBA_octet_serialize(cdrCodec,&(object->door_back));
- CORBA_octet_serialize(cdrCodec,&(object->reserve));
-}
-
-void
-servos_deserialize(CDR_Codec *cdrCodec,servos *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->brush_left));
- CORBA_octet_deserialize(cdrCodec,&(object->brush_right));
- CORBA_octet_deserialize(cdrCodec,&(object->door_bottom));
- CORBA_octet_deserialize(cdrCodec,&(object->door_top));
- CORBA_octet_deserialize(cdrCodec,&(object->door_back));
- CORBA_octet_deserialize(cdrCodec,&(object->reserve));
-}
-
-int
-servos_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-servos_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "servos",
- (ORTETypeSerialize)servos_serialize,
- (ORTETypeDeserialize)servos_deserialize,
- servos_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - drives */
-/****************************************************************/
-
-void drives_serialize(CDR_Codec *cdrCodec,drives *object) {
- CORBA_octet_serialize(cdrCodec,&(object->brush_left));
- CORBA_octet_serialize(cdrCodec,&(object->brush_right));
- CORBA_octet_serialize(cdrCodec,&(object->bagr));
- CORBA_octet_serialize(cdrCodec,&(object->carousel_pos));
-}
-
-void
-drives_deserialize(CDR_Codec *cdrCodec,drives *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->brush_left));
- CORBA_octet_deserialize(cdrCodec,&(object->brush_right));
- CORBA_octet_deserialize(cdrCodec,&(object->bagr));
- CORBA_octet_deserialize(cdrCodec,&(object->carousel_pos));
-}
-
-int
-drives_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-drives_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "drives",
- (ORTETypeSerialize)drives_serialize,
- (ORTETypeDeserialize)drives_deserialize,
- drives_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - laser_cmd */
-/****************************************************************/
-
-void laser_cmd_serialize(CDR_Codec *cdrCodec,laser_cmd *object) {
- CORBA_octet_serialize(cdrCodec,&(object->speed));
-}
-
-void
-laser_cmd_deserialize(CDR_Codec *cdrCodec,laser_cmd *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->speed));
-}
-
-int
-laser_cmd_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-laser_cmd_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "laser_cmd",
- (ORTETypeSerialize)laser_cmd_serialize,
- (ORTETypeDeserialize)laser_cmd_deserialize,
- laser_cmd_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - laser_data */
-/****************************************************************/
-
-void laser_data_serialize(CDR_Codec *cdrCodec,laser_data *object) {
- CORBA_unsigned_short_serialize(cdrCodec,&(object->period));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measure));
-}
-
-void
-laser_data_deserialize(CDR_Codec *cdrCodec,laser_data *object) {
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->period));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measure));
-}
-
-int
-laser_data_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-laser_data_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "laser_data",
- (ORTETypeSerialize)laser_data_serialize,
- (ORTETypeDeserialize)laser_data_deserialize,
- laser_data_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - laser_meas */
-/****************************************************************/
-
-void laser_meas_serialize(CDR_Codec *cdrCodec,laser_meas *object) {
- CORBA_unsigned_short_serialize(cdrCodec,&(object->cnt));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->period));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures0));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures1));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures2));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures3));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures4));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures5));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures6));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures7));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures8));
- CORBA_unsigned_short_serialize(cdrCodec,&(object->measures9));
-}
-
-void
-laser_meas_deserialize(CDR_Codec *cdrCodec,laser_meas *object) {
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->cnt));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->period));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures0));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures1));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures2));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures3));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures4));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures5));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures6));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures7));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures8));
- CORBA_unsigned_short_deserialize(cdrCodec,&(object->measures9));
-}
-
-int
-laser_meas_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- CORBA_unsigned_short_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-laser_meas_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "laser_meas",
- (ORTETypeSerialize)laser_meas_serialize,
- (ORTETypeDeserialize)laser_meas_deserialize,
- laser_meas_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - sharps */
-/****************************************************************/
-
-void sharps_serialize(CDR_Codec *cdrCodec,sharps *object) {
- CORBA_double_serialize(cdrCodec,&(object->left));
- CORBA_double_serialize(cdrCodec,&(object->right));
- CORBA_double_serialize(cdrCodec,&(object->front_left));
- CORBA_double_serialize(cdrCodec,&(object->front_right));
- CORBA_double_serialize(cdrCodec,&(object->back_left));
- CORBA_double_serialize(cdrCodec,&(object->back_right));
-}
-
-void
-sharps_deserialize(CDR_Codec *cdrCodec,sharps *object) {
- CORBA_double_deserialize(cdrCodec,&(object->left));
- CORBA_double_deserialize(cdrCodec,&(object->right));
- CORBA_double_deserialize(cdrCodec,&(object->front_left));
- CORBA_double_deserialize(cdrCodec,&(object->front_right));
- CORBA_double_deserialize(cdrCodec,&(object->back_left));
- CORBA_double_deserialize(cdrCodec,&(object->back_right));
-}
-
-int
-sharps_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- CORBA_double_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-sharps_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "sharps",
- (ORTETypeSerialize)sharps_serialize,
- (ORTETypeDeserialize)sharps_deserialize,
- sharps_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - cmu */
-/****************************************************************/
-
-void cmu_serialize(CDR_Codec *cdrCodec,cmu *object) {
- CORBA_octet_serialize(cdrCodec,&(object->color));
-}
-
-void
-cmu_deserialize(CDR_Codec *cdrCodec,cmu *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->color));
-}
-
-int
-cmu_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-cmu_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "cmu",
- (ORTETypeSerialize)cmu_serialize,
- (ORTETypeDeserialize)cmu_deserialize,
- cmu_get_max_size,
- 0);
- return ret;
-}
-
-/****************************************************************/
-/* struct - bumper */
-/****************************************************************/
-
-void bumper_serialize(CDR_Codec *cdrCodec,bumper *object) {
- CORBA_octet_serialize(cdrCodec,&(object->left));
- CORBA_octet_serialize(cdrCodec,&(object->right));
-}
-
-void
-bumper_deserialize(CDR_Codec *cdrCodec,bumper *object) {
- CORBA_octet_deserialize(cdrCodec,&(object->left));
- CORBA_octet_deserialize(cdrCodec,&(object->right));
-}
-
-int
-bumper_get_max_size(ORTEGetMaxSizeParam *gms) {
- CORBA_octet_get_max_size(gms);
- CORBA_octet_get_max_size(gms);
- return gms->csize;
-}
-
-Boolean
-bumper_type_register(ORTEDomain *d) {
- Boolean ret;
-
- ret=ORTETypeRegisterAdd(d,
- "bumper",
- (ORTETypeSerialize)bumper_serialize,
- (ORTETypeDeserialize)bumper_deserialize,
- bumper_get_max_size,
- 0);
- return ret;
-}
-
+++ /dev/null
-/*
- * This file was generated by orte-idl - DO NOT EDIT!
- */
-
-#ifndef robottype_eb2008_H
-#define robottype_eb2008_H 1
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-#ifndef EXCLUDE_ORTE_H
-#include <orte.h>
-#endif /* EXCLUDE_ORTE_H */
-
-/** typedefs **/
-#if !defined(_servos_defined)
-#define _servos_defined 1
-typedef struct servos_type servos;
-struct servos_type {
-CORBA_octet brush_left;
-CORBA_octet brush_right;
-CORBA_octet door_bottom;
-CORBA_octet door_top;
-CORBA_octet door_back;
-CORBA_octet reserve;
-};
-
-#endif
-#if !defined(_drives_defined)
-#define _drives_defined 1
-typedef struct drives_type drives;
-struct drives_type {
-CORBA_octet brush_left;
-CORBA_octet brush_right;
-CORBA_octet bagr;
-CORBA_octet carousel_pos;
-};
-
-#endif
-#if !defined(_laser_cmd_defined)
-#define _laser_cmd_defined 1
-typedef struct laser_cmd_type laser_cmd;
-struct laser_cmd_type {
-CORBA_octet speed;
-};
-
-#endif
-#if !defined(_laser_data_defined)
-#define _laser_data_defined 1
-typedef struct laser_data_type laser_data;
-struct laser_data_type {
-CORBA_unsigned_short period;
-CORBA_unsigned_short measure;
-};
-
-#endif
-#if !defined(_laser_meas_defined)
-#define _laser_meas_defined 1
-typedef struct laser_meas_type laser_meas;
-struct laser_meas_type {
-CORBA_unsigned_short cnt;
-CORBA_unsigned_short period;
-CORBA_unsigned_short measures0;
-CORBA_unsigned_short measures1;
-CORBA_unsigned_short measures2;
-CORBA_unsigned_short measures3;
-CORBA_unsigned_short measures4;
-CORBA_unsigned_short measures5;
-CORBA_unsigned_short measures6;
-CORBA_unsigned_short measures7;
-CORBA_unsigned_short measures8;
-CORBA_unsigned_short measures9;
-};
-
-#endif
-#if !defined(_sharps_defined)
-#define _sharps_defined 1
-typedef struct sharps_type sharps;
-struct sharps_type {
-CORBA_double left;
-CORBA_double right;
-CORBA_double front_left;
-CORBA_double front_right;
-CORBA_double back_left;
-CORBA_double back_right;
-};
-
-#endif
-#if !defined(_cmu_defined)
-#define _cmu_defined 1
-typedef struct cmu_type cmu;
-struct cmu_type {
-CORBA_octet color;
-};
-
-#endif
-#if !defined(_bumper_defined)
-#define _bumper_defined 1
-typedef struct bumper_type bumper;
-struct bumper_type {
-CORBA_octet left;
-CORBA_octet right;
-};
-
-#endif
-
-/** impls declarations **/
-void servos_serialize(CDR_Codec *cdrCodec,servos *object);
-void servos_deserialize(CDR_Codec *cdrCodec,servos *object);
-int servos_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean servos_type_register(ORTEDomain *d);
-
-void drives_serialize(CDR_Codec *cdrCodec,drives *object);
-void drives_deserialize(CDR_Codec *cdrCodec,drives *object);
-int drives_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean drives_type_register(ORTEDomain *d);
-
-void laser_cmd_serialize(CDR_Codec *cdrCodec,laser_cmd *object);
-void laser_cmd_deserialize(CDR_Codec *cdrCodec,laser_cmd *object);
-int laser_cmd_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean laser_cmd_type_register(ORTEDomain *d);
-
-void laser_data_serialize(CDR_Codec *cdrCodec,laser_data *object);
-void laser_data_deserialize(CDR_Codec *cdrCodec,laser_data *object);
-int laser_data_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean laser_data_type_register(ORTEDomain *d);
-
-void laser_meas_serialize(CDR_Codec *cdrCodec,laser_meas *object);
-void laser_meas_deserialize(CDR_Codec *cdrCodec,laser_meas *object);
-int laser_meas_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean laser_meas_type_register(ORTEDomain *d);
-
-void sharps_serialize(CDR_Codec *cdrCodec,sharps *object);
-void sharps_deserialize(CDR_Codec *cdrCodec,sharps *object);
-int sharps_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean sharps_type_register(ORTEDomain *d);
-
-void cmu_serialize(CDR_Codec *cdrCodec,cmu *object);
-void cmu_deserialize(CDR_Codec *cdrCodec,cmu *object);
-int cmu_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean cmu_type_register(ORTEDomain *d);
-
-void bumper_serialize(CDR_Codec *cdrCodec,bumper *object);
-void bumper_deserialize(CDR_Codec *cdrCodec,bumper *object);
-int bumper_get_max_size(ORTEGetMaxSizeParam *gms);
-Boolean bumper_type_register(ORTEDomain *d);
-
-
-#ifdef __cplusplus
-}
-#endif /* __cplusplus */
-
-#endif
+++ /dev/null
-struct servos {
- octet brush_left;
- octet brush_right;
- octet door_bottom;
- octet door_top;
- octet door_back;
- octet reserve;
-};
-
-struct drives {
- octet brush_left;
- octet brush_right;
- octet bagr;
- octet carousel_pos;
-};
-
-struct laser_cmd {
- octet speed;
-};
-
-struct laser_data {
- unsigned short period;
- unsigned short measure;
-};
-
-struct laser_meas {
- unsigned short cnt;
- unsigned short period;
- unsigned short measures0;
- unsigned short measures1;
- unsigned short measures2;
- unsigned short measures3;
- unsigned short measures4;
- unsigned short measures5;
- unsigned short measures6;
- unsigned short measures7;
- unsigned short measures8;
- unsigned short measures9;
-};
-
-struct sharps {
- double left;
- double right;
- double front_left;
- double front_right;
- double back_left;
- double back_right;
-};
-
-struct cmu {
- octet color;
-};
-
-struct bumper {
- octet left;
- octet right;
-};
#include <string.h>
#include <robottype.h>
-#include <robottype_eb2008.h>
-#include <robot_eb2008.h>
+#include <robottype.h>
+#include <robot.h>
#include "oledlib.h"
#include <stdint.h>
#include <robottype.h>
-#include <robottype_eb2008.h>
+#include <robottype.h>
#define HW_STATUS_MSG_SIZE 9
#define SWITCH_MODE_MSG_SIZE 5
#include <stdlib.h>
#include <stdio.h>
#include <robottype.h>
-#include <robottype_eb2008.h>
+#include <robottype.h>
#include "uoled.h"
#include "uoled_config.h"
projects / eurobot / public.git / commitdiff