#include <can_ids.h>
#include <sharp.h>
#include <orte.h>
-#include <roboorte_eb2007.h>
+#include <roboorte_eb2008.h>
#include "cand_eb2008.h"
return 0;
}
-int set_motor_speed(struct 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 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 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;
+ /* voltage measurements from power board */
case CAN_PWR_ADC1:
double volt80, voltBAT;
volt80 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
- (frame.data[2] << 8) | (frame.data[3]))/1000.0;
+ (frame.data[2] << 8) | (frame.data[3]))/10000.0;
voltBAT = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
- (frame.data[6] << 8) | (frame.data[7]))/1000.0;
+ (frame.data[6] << 8) | (frame.data[7]))/10000.0;
printf("PWR_ADC1: 8,0V = %2.2f, BAT = %2.2f\n",volt80,voltBAT);
-
+ orte->pwr_voltage.voltage80 = volt80;
+ orte->pwr_voltage.voltageBAT = voltBAT;
break;
case CAN_PWR_ADC2:
double volt33, volt50;
volt33 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
- (frame.data[2] << 8) | (frame.data[3]))/1000.0;
+ (frame.data[2] << 8) | (frame.data[3]))/10000.0;
volt50 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
- (frame.data[6] << 8) | (frame.data[7]))/1000.0;
+ (frame.data[6] << 8) | (frame.data[7]))/10000.0;
printf("PWR_ADC2: 3,3V = %2.2f, 5,0V = %2.2f\n",volt33,volt50);
+ orte->pwr_voltage.voltage33 = volt33;
+ orte->pwr_voltage.voltage50 = volt50;
+ ORTEPublicationSend(orte->publication_pwr_voltage);
+
break;
default:
// printf("received CAN msg with unknown id: %x\n",frame.can_id);
}
}
-void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
-{
- struct orte_data *orte_data = (struct 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 orte_data *orte_data = (struct 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[])
{
roboorte_init(NULL, &orte);
/* publishers registration */
roboorte_register_publisher(&orte,
- ORTE_PUB_MOTSTAT | ORTE_PUB_POSITION |
- ORTE_PUB_FD | ORTE_PUB_IR |
- ORTE_PUB_SHARPLONG | ORTE_PUB_SHARPSHORT |
- ORTE_PUB_DI | ORTE_PUB_ADCS | ORTE_PUB_LASER);
+ ORTE_PUB_PWR_VOLT);
/* subscribers registration */
- orte.rcv_motion_speed_cb = rcv_motion_speed_cb;
- orte.rcv_servos_cb = rcv_servos_cb;
-
- roboorte_register_subscriber(&orte,
- ORTE_SUB_MOTOR | ORTE_SUB_SERVOS);
-
for(;;) {
FD_ZERO(&read_fd_set);
FD_SET(sock, &read_fd_set);
# -*- makefile -*-
# Robot's ORTE library
-#lib_LIBRARIES += roboorte_eb2008
-#roboorte_eb2008_SOURCES = roboorte_eb2008.c
-#include_HEADERS += roboorte_eb2008.h
+lib_LIBRARIES += roboorte_eb2008
+roboorte_eb2008_SOURCES = roboorte_eb2008.c
+include_HEADERS += roboorte_eb2008.h
--- /dev/null
+/*
+ * roboorte_eb2008.c 08/04/02
+ *
+ * Robot's ORTE library. Used for Eurobot 2007.
+ *
+ * Copyright: (c) 2007 DCE Eurobot Dragon Team
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#include "roboorte_eb2008.h"
+
+/* ----------------------------------------------------------------------
+ * PUBLISHERS
+ * ---------------------------------------------------------------------- */
+
+void *publisher_pwr_voltage_create(void *arg, struct orte_data *data)
+{
+ NtpTime persistence, delay;
+
+ pwr_act_volts_type_register(data->orte_domain);
+
+ NTPTIME_BUILD(persistence, 3);
+ NtpTimeAssembFromMs(delay, 0, 100);
+ data->publication_pwr_voltage = ORTEPublicationCreate(
+ data->orte_domain, "pwr_voltage", "pwr_act_volts",
+ &data->pwr_voltage, &persistence,
+ 1, NULL, NULL, &delay);
+ return arg;
+}
+
+void *publisher_pwr_ctrl_create(void *arg, struct orte_data *data)
+{
+ NtpTime persistence, delay;
+
+ pwr_ctrl_type_register(data->orte_domain);
+
+ NTPTIME_BUILD(persistence, 3);
+ NtpTimeAssembFromMs(delay, 0, 100);
+ data->publication_pwr_ctrl = ORTEPublicationCreate(
+ data->orte_domain, "pwr_ctrl", "pwr_ctrl",
+ &data->pwr_ctrl, &persistence,
+ 1, NULL, NULL, &delay);
+ return arg;
+}
+
+
+/* ----------------------------------------------------------------------
+ * SUBSCRIBERS
+ * ---------------------------------------------------------------------- */
+
+void *subscriber_pwr_voltage_create(void *arg, struct orte_data *data)
+{
+ ORTESubscription *s;
+ NtpTime deadline, minimumSeparation;
+
+ pwr_act_volts_type_register(data->orte_domain);
+
+ NTPTIME_BUILD(deadline, 10);
+ NTPTIME_BUILD(minimumSeparation, 0);
+ s = ORTESubscriptionCreate(
+ data->orte_domain, IMMEDIATE, BEST_EFFORTS,
+ "pwr_voltage", "pwr_act_volts",
+ &data->pwr_voltage, &deadline,
+ &minimumSeparation, data->rcv_pwr_voltage_cb,
+ data, IPADDRESS_INVALID);
+ return arg;
+}
+
+void *subscriber_pwr_ctrl_create(void *arg, struct orte_data *data)
+{
+ ORTESubscription *s;
+ NtpTime deadline, minimumSeparation;
+
+ pwr_ctrl_type_register(data->orte_domain);
+
+ NTPTIME_BUILD(deadline, 10);
+ NTPTIME_BUILD(minimumSeparation, 0);
+ s = ORTESubscriptionCreate(
+ data->orte_domain, IMMEDIATE, BEST_EFFORTS,
+ "pwr_ctrl", "pwr_ctrl",
+ &data->pwr_voltage, &deadline,
+ &minimumSeparation, data->rcv_pwr_ctrl_cb,
+ data, IPADDRESS_INVALID);
+ return arg;
+}
+
+
+int roboorte_register_publisher(struct orte_data *data, u_int32_t publishers)
+{
+ int i;
+
+ for (i=0; i<32; i++) {
+ switch (publishers & ORTE_MASK(i)) {
+ case ORTE_PUB_PWR_VOLT:
+ publisher_pwr_voltage_create(NULL, data);
+ break;
+ case ORTE_PUB_PWR_CTRL:
+ publisher_pwr_ctrl_create(NULL, data);
+ break;
+ }
+ }
+
+ return 0;
+}
+
+int roboorte_register_subscriber(struct orte_data *data, u_int32_t subscribers)
+{
+ int i;
+
+ for (i=0; i<32; i++) {
+ switch (subscribers & ORTE_MASK(i)) {
+ case ORTE_SUB_PWR_VOLT:
+ subscriber_pwr_voltage_create(NULL, data);
+ break;
+ case ORTE_SUB_PWR_CTRL:
+ subscriber_pwr_voltage_create(NULL, data);
+ break;
+
+ }
+ }
+
+ return 0;
+}
+
+
+/* inilization callback function */
+void on_orte_reg_fail(void *param)
+{
+ printf("registration to a manager failed\n");
+}
+
+int roboorte_init(void *arg, struct orte_data *data)
+{
+ int rv = 0;
+ ORTEDomainAppEvents events;
+
+ ORTEInit();
+
+ ORTEDomainInitEvents(&events);
+ events.onRegFail = on_orte_reg_fail;
+
+ ORTEVerbositySetOptions("ALL.0");
+ data->orte_domain = ORTEDomainAppCreate(ORTE_DEFAULT_DOMAIN,
+ NULL, NULL, ORTE_FALSE);
+ if (!data->orte_domain) {
+ printf("ORTEDomainAppCreate failed!\n");
+ rv = -1;
+ }
+
+ return rv;
+}
--- /dev/null
+/*
+ * roboorte_eb2008.h 08/04/02
+ *
+ * Robot's ORTE library. Used for Eurobot 2007.
+ *
+ * Copyright: (c) 2007 DCE Eurobot Dragon Team
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#ifndef ROBOORTE_EB2008_H
+#define ROBOORTE_EB2008_H
+
+#include <orte.h>
+#include <RobotType.h>
+#include <RobotTypeArr.h>
+#include <stdio.h>
+#include <sys/types.h>
+
+/* publishers & subscribers bit mask */
+#define ORTE_MASK(x) ((u_int32_t)1 << (x))
+
+/* publishers */
+#define ORTE_PUB_PWR_VOLT ORTE_MASK(0)
+#define ORTE_PUB_PWR_CTRL ORTE_MASK(1)
+
+/* subscribers */
+#define ORTE_SUB_PWR_VOLT ORTE_MASK(0)
+#define ORTE_SUB_PWR_CTRL ORTE_MASK(1)
+/* ORTE data */
+struct orte_data {
+ ORTEDomain *orte_domain;
+
+ struct pwr_act_volts_type pwr_voltage;
+ struct pwr_ctrl_type pwr_ctrl;
+
+ /* publishers */
+ ORTEPublication *publication_pwr_voltage;
+ ORTEPublication *publication_pwr_ctrl;
+
+ /* subscribers */
+ ORTERecvCallBack rcv_pwr_voltage_cb;
+ ORTERecvCallBack rcv_pwr_ctrl_cb;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+int roboorte_init(void *arg, struct orte_data *data);
+int roboorte_register_publisher(struct orte_data *data, u_int32_t publishers);
+int roboorte_register_subscriber(struct orte_data *data, u_int32_t subscribers);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ROBOORTE_EB2008_H */
return ret;
}
+/****************************************************************/
+/* struct - pwr_act_volts */
+/****************************************************************/
+
+void pwr_act_volts_serialize(CDR_Codec *cdrCodec,pwr_act_volts *object) {
+ CORBA_double_serialize(cdrCodec,&(object->voltage33));
+ CORBA_double_serialize(cdrCodec,&(object->voltage50));
+ CORBA_double_serialize(cdrCodec,&(object->voltage80));
+ CORBA_double_serialize(cdrCodec,&(object->voltageBAT));
+}
+
+void
+pwr_act_volts_deserialize(CDR_Codec *cdrCodec,pwr_act_volts *object) {
+ CORBA_double_deserialize(cdrCodec,&(object->voltage33));
+ CORBA_double_deserialize(cdrCodec,&(object->voltage50));
+ CORBA_double_deserialize(cdrCodec,&(object->voltage80));
+ CORBA_double_deserialize(cdrCodec,&(object->voltageBAT));
+}
+
+int
+pwr_act_volts_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);
+ return gms->csize;
+}
+
+Boolean
+pwr_act_volts_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "pwr_act_volts",
+ (ORTETypeSerialize)pwr_act_volts_serialize,
+ (ORTETypeDeserialize)pwr_act_volts_deserialize,
+ pwr_act_volts_get_max_size,
+ 0);
+ return ret;
+}
+
+/****************************************************************/
+/* struct - pwr_ctrl */
+/****************************************************************/
+
+void pwr_ctrl_serialize(CDR_Codec *cdrCodec,pwr_ctrl *object) {
+ CORBA_boolean_serialize(cdrCodec,&(object->voltage33));
+ CORBA_boolean_serialize(cdrCodec,&(object->voltage50));
+ CORBA_boolean_serialize(cdrCodec,&(object->voltage80));
+ CORBA_boolean_serialize(cdrCodec,&(object->voltageBAT));
+}
+
+void
+pwr_ctrl_deserialize(CDR_Codec *cdrCodec,pwr_ctrl *object) {
+ CORBA_boolean_deserialize(cdrCodec,&(object->voltage33));
+ CORBA_boolean_deserialize(cdrCodec,&(object->voltage50));
+ CORBA_boolean_deserialize(cdrCodec,&(object->voltage80));
+ CORBA_boolean_deserialize(cdrCodec,&(object->voltageBAT));
+}
+
+int
+pwr_ctrl_get_max_size(ORTEGetMaxSizeParam *gms) {
+ CORBA_boolean_get_max_size(gms);
+ CORBA_boolean_get_max_size(gms);
+ CORBA_boolean_get_max_size(gms);
+ CORBA_boolean_get_max_size(gms);
+ return gms->csize;
+}
+
+Boolean
+pwr_ctrl_type_register(ORTEDomain *d) {
+ Boolean ret;
+
+ ret=ORTETypeRegisterAdd(d,
+ "pwr_ctrl",
+ (ORTETypeSerialize)pwr_ctrl_serialize,
+ (ORTETypeDeserialize)pwr_ctrl_deserialize,
+ pwr_ctrl_get_max_size,
+ 0);
+ return ret;
+}
+
CORBA_unsigned_short position4;
};
+#endif
+#if !defined(_pwr_act_volts_defined)
+#define _pwr_act_volts_defined 1
+typedef struct pwr_act_volts_type pwr_act_volts;
+struct pwr_act_volts_type {
+CORBA_double voltage33;
+CORBA_double voltage50;
+CORBA_double voltage80;
+CORBA_double voltageBAT;
+};
+
+#endif
+#if !defined(_pwr_ctrl_defined)
+#define _pwr_ctrl_defined 1
+typedef struct pwr_ctrl_type pwr_ctrl;
+struct pwr_ctrl_type {
+CORBA_boolean voltage33;
+CORBA_boolean voltage50;
+CORBA_boolean voltage80;
+CORBA_boolean voltageBAT;
+};
+
#endif
/** impls declarations **/
int accumulator_get_max_size(ORTEGetMaxSizeParam *gms);
Boolean accumulator_type_register(ORTEDomain *d);
+void pwr_act_volts_serialize(CDR_Codec *cdrCodec,pwr_act_volts *object);
+void pwr_act_volts_deserialize(CDR_Codec *cdrCodec,pwr_act_volts *object);
+int pwr_act_volts_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean pwr_act_volts_type_register(ORTEDomain *d);
+
+void pwr_ctrl_serialize(CDR_Codec *cdrCodec,pwr_ctrl *object);
+void pwr_ctrl_deserialize(CDR_Codec *cdrCodec,pwr_ctrl *object);
+int pwr_ctrl_get_max_size(ORTEGetMaxSizeParam *gms);
+Boolean pwr_ctrl_type_register(ORTEDomain *d);
+
#ifdef __cplusplus
}
unsigned short position3;
unsigned short position4;
};
+
+struct pwr_act_volts {
+ double voltage33;
+ double voltage50;
+ double voltage80;
+ double voltageBAT;
+};
+
+struct pwr_ctrl {
+ boolean voltage33;
+ boolean voltage50;
+ boolean voltage80;
+ boolean voltageBAT;
+};
projects / eurobot / public.git / commitdiff