--- /dev/null
+/*
+ * robot_orte.c 08/04/21
+ *
+ * Robot's orte stuffs.
+ *
+ * Copyright: (c) 2008 CTU Dragons
+ * CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#include <orte.h>
+#include <roboorte_generic.h>
+#include <roboorte_eb2008.h>
+#include <robodata.h>
+#include <robot_eb2008.h>
+#include <movehelper_eb2008.h>
+#include <math.h>
+#include <robomath.h>
+#include <laser-nav.h>
+
+/* ----------------------------------------------------------------------
+ * PUBLISHER CALLBACKS - GENERIC
+ * ---------------------------------------------------------------------- */
+
+void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
+
+ ROBOT_LOCK(ref_pos);
+ *instance = robot.ref_pos;
+ ROBOT_UNLOCK(ref_pos);
+}
+
+void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+ struct est_pos_type *instance = (struct est_pos_type *)vinstance;
+
+ ROBOT_LOCK(est_pos);
+ *instance = robot.est_pos;
+ ROBOT_UNLOCK(est_pos);
+}
+
+void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
+ void *sendCallBackParam)
+{
+}
+
+/* ----------------------------------------------------------------------
+ * SUBSCRIBER CALLBACKS - GENERIC
+ * ---------------------------------------------------------------------- */
+
+void rcv_motion_irc_cb(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;
+ /* spocitat prevodovy pomer */
+ double n = (double)(28.0 / 1.0);
+ /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
+ double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
+
+ double aktk0 = 0;
+ double aktk1 = 0;
+ double deltaU = 0;
+ double deltAlfa = 0;
+
+ switch (info->status) {
+ case NEW_DATA:
+ if(firstRun) {
+ prevInstance = *instance;
+ firstRun = 0;
+ break;
+ }
+
+ aktk0 = ((prevInstance.left - instance->left) >> 8) * c;
+ aktk1 = ((instance->right - prevInstance.right) >> 8) * c;
+ prevInstance = *instance;
+
+ deltaU = (aktk0 + aktk1) / 2;
+ deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
+
+ struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
+ odo->dx = deltaU;
+ odo->dy = 0;
+ odo->dangle = deltAlfa;
+ FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo);
+ hw_status[STATUS_MOTION] = HW_STATUS_OK;
+ break;
+ case DEADLINE:
+ hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
+ printf("ORTE deadline occurred - motion_irc receive\n");
+ break;
+ }
+}
+
+void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ switch (info->status) {
+ case NEW_DATA:
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - motion_speed receive\n");
+ break;
+ }
+}
+
+void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ switch (info->status) {
+ case NEW_DATA:
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - motion_status receive\n");
+ break;
+ }
+}
+
+void rcv_joy_data_cb(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_data);
+ robot.joy_data = *instance;
+ ROBOT_UNLOCK(joy_data);
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - joy_data receive\n");
+ break;
+ }
+}
+
+void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ switch (info->status) {
+ case NEW_DATA:
+ hw_status[STATUS_POWER] = HW_STATUS_OK;
+ break;
+ case DEADLINE:
+ hw_status[STATUS_POWER] = HW_STATUS_FAILED;
+ printf("ORTE deadline occurred - pwr_voltage receive\n");
+ break;
+ }
+}
+
+void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ switch (info->status) {
+ case NEW_DATA:
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - pwr_ctrl receive\n");
+ break;
+ }
+}
+
+/* ----------------------------------------------------------------------
+ * SUBSCRIBER CALLBACKS - EB2008
+ * ---------------------------------------------------------------------- */
+
+void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ switch (info->status) {
+ case NEW_DATA:
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - servos receive\n");
+ break;
+ }
+}
+
+void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct drives_type *instance = (struct drives_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA:
+ ROBOT_LOCK(drives);
+ robot.drives = *instance;
+ ROBOT_UNLOCK(drives);
+ struct drives_type *drives =
+ malloc(sizeof(struct drives_type));
+ *drives = *instance;
+ FSM_SIGNAL(MAIN, EV_CAROUSEL_POS_ACHIEVED, drives);
+ break;
+ case DEADLINE:
+ printf("ORTE deadline occurred - drives receive\n");
+ break;
+ }
+}
+
+void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct laser_data_type *instance = (struct laser_data_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ struct mcl_laser_measurement *meas_angles;
+ meas_angles = malloc(sizeof(*meas_angles));
+
+ meas_angles->count = 1;
+ meas_angles->val[0] =
+ (double)TIME2ANGLE(instance->period,
+ instance->measure);
+// printf("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
+
+ bool sent;
+ FSM_SIGNAL_TIMED(LOC, EV_LASER_RECEIVED, meas_angles, 20/*ms*/, &sent);
+ if (!sent) {
+ printf("%s: MCL is busy - not updating\n", __FUNCTION__);
+ free(meas_angles);
+ }
+ hw_status[STATUS_LASER] = HW_STATUS_OK;
+ break;
+ }
+ case DEADLINE:
+ hw_status[STATUS_LASER] = HW_STATUS_FAILED;
+ printf("%s: ORTE deadline occurred\n", __FUNCTION__);
+ break;
+ }
+}
+
+void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct cmu_type *instance = (struct cmu_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ ROBOT_LOCK(cmu);
+ robot.cmu = *instance;
+ ROBOT_UNLOCK(cmu);
+ if (robot.cmu.color != NO_BALL)
+ FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
+ break;
+ hw_status[STATUS_CMU] = HW_STATUS_OK;
+ }
+ case DEADLINE:
+ hw_status[STATUS_CMU] = HW_STATUS_FAILED;
+ printf("%s: ORTE deadline occurred\n", __FUNCTION__);
+ break;
+ }
+}
+
+void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct bumper_type *instance = (struct bumper_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ ROBOT_LOCK(bumper);
+ robot.bumper = *instance;
+ ROBOT_UNLOCK(bumper);
+ break;
+ }
+ case DEADLINE:
+ printf("%s: ORTE deadline occurred\n", __FUNCTION__);
+ break;
+ }
+}
+
+void robot_init_orte()
+{
+ robot.orte.strength = 20;
+ robot.gorte.strength = 20;
+
+ eb2008_roboorte_init(&robot.orte);
+ generic_roboorte_init(&robot.gorte);
+
+ /* 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);
+
+ /* 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);
+
+ /* 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);
+}
+
projects / eurobot / public.git / commitdiff