bin_PROGRAMS = rozpuk camgrab
-rozpuk_SOURCES = rozpuk.c camera.c decjpeg.c hue.c likelimg.c
-rozpuk_LIBS = jpeg robodim
+rozpuk_SOURCES = rozpuk.c camera.c decjpeg.c hue.c likelimg.c rozporte.c
+rozpuk_LIBS = jpeg robodim pthread roboorte robottype robottype orte
camgrab_SOURCES = camgrab.c camera.c
\ No newline at end of file
for (ip = ib = 0; ip < length; ib++) {
bitmap[ib] = 0;
for (b = 1<<7; b; b >>= 1, ip++)
- if ((img[ip] < t1) || (img[ip] > t2))
+ if ((t2 > t1)
+ ? ((img[ip] < t1) || (img[ip] > t2))
+ : ((img[ip] > t2) && (img[ip] < t1)))
bitmap[ib] += b;
}
}
#include <inttypes.h>
#include <stdlib.h>
#include <stdio.h>
+#include <semaphore.h>
#include <getopt.h>
#include <robodim.h>
#include "decjpeg.h"
#include "hue.h"
#include "likelimg.h"
+#include "rozporte.h"
-#define N_LOTS 10
-#define HUE_THR_LOW 106
-#define HUE_THR_HIGH 235
+#define N_LOTS 10
+#define DEFAULT_HUE_THR_FROM 106
+#define DEFAULT_HUE_THR_TO 235
#define HUE_SIZE (IMG_WIDTH*IMG_HEIGHT)
#define RGB_SIZE (2*HUE_SIZE*3)
pdfmask_t pdf_green[N_SPOTS], pdf_red[N_SPOTS];
+uint8_t hue_thr_from = DEFAULT_HUE_THR_FROM, hue_thr_to = DEFAULT_HUE_THR_TO;
uint8_t rgb[RGB_SIZE], hue[HUE_SIZE];
char bitmap[HUE_SIZE/8];
+/* show opts */
+int show_skip = 15, show_rgb = 0, show_proc = 0;
void process_jpeg_frame(void *jpeg) {
jpeg_decode(jpeg, JPEG_SIZE, rgb, RGB_SIZE, NULL, NULL);
}
-int rozpuk(pdfmask_t *pdf) {
+int recognize_lot(pdfmask_t *pdf) {
double W[N_SPOTS], L[N_LOTS], max = -1;
int i, j, k, lot = 0;
char b;
return lot;
}
+void rozpuk() {
+ int lot;
+
+ sem_init(&running_sem, 0, 0);
+
+ for (;;) {
+ while (camera_on("/dev/video0", 640, 480, V4L2_PIX_FMT_MJPEG)) {
+ perror("camera_on");
+ usleep(500*1000);
+ }
+
+ while (running) {
+ camera_get_frame(process_jpeg_frame, 1000 /* msec */);
+ image_hue(rgb, hue, 320*480);
+ image_threshold(hue, bitmap, 320*480, hue_thr_from, hue_thr_to);
+ lot = recognize_lot((game_color == 0) ? pdf_green : pdf_red);
+ orte_send(lot);
+ printf("(%s) %2d\n", game_color?"RED":"GREEN", 1+lot);
+ }
+
+ camera_off();
+ while (!running)
+ sem_wait(&running_sem);
+ }
+}
+
+/* debug: grab/process image */
+void show() {
+ int i, j, k, tmp, lot;
+
+ while (camera_on("/dev/video0", 640, 480, V4L2_PIX_FMT_MJPEG)) {
+ perror("camera_on");
+ usleep(500*1000);
+ }
+
+ for (i = 0; i < show_skip; i++)
+ camera_get_frame(NULL, 0);
+
+ camera_get_frame(process_jpeg_frame, 0);
+ image_hue(rgb, hue, 320*480);
+ image_threshold(hue, bitmap, 320*480, hue_thr_from, hue_thr_to);
+
+ if (show_rgb) {
+ printf("P6\n640 480\n255\n");
+ fwrite(rgb, 640*480*3, 1, stdout);
+ }
+ if (show_proc) {
+ printf("P6\n320 480\n255\n");
+ for (i = j = 0; i < 320*480; i++, j += 3) {
+ rgb[j] = hue[i];
+ rgb[j+1] = 0;
+ rgb[j+2] = (bitmap[i/8] & (1<<(7-(i%8)))) ? 255 : 0;
+ }
+ fwrite(rgb, 320*480*3, 1, stdout);
+ }
+
+ lot = recognize_lot((game_color == 0) ? pdf_green : pdf_red);
+ fprintf(stderr, "%2d\n", 1+lot);
+}
+
int main(int argc, char *argv[]) {
FILE *f;
- int i, lot;
- int do_grab = 0, do_hue = 0;
+ int opt, tmp, do_show;
- if (argc > 1) {
- switch (argv[1][0]) {
- case 'g':
- do_grab = 1;
- break;
+ while ((opt = getopt(argc, argv, "c:s:rp")) != -1)
+ switch (opt) {
case 'c':
- do_hue = 1;
+ game_color = atoi(optarg);
+ fprintf(stderr, "selected %s game color\n", game_color?"RED":"GREEN");
+ break;
+ case 's':
+ if (optarg != NULL)
+ show_skip = atoi(optarg);
+ break;
+ case 'r':
+ show_rgb = 1;
+ break;
+ case 'p':
+ show_proc = 1;
break;
}
+ if (optind + 1 < argc) {
+ tmp = atoi(argv[optind]);
+ if ((tmp >= 0) && (tmp < 256))
+ hue_thr_from = (uint8_t)tmp;
+ tmp = atoi(argv[optind+1]);
+ if ((tmp >= 0) && (tmp < 256))
+ hue_thr_to = (uint8_t)tmp;
+ fprintf(stderr, "hue thr: %d/%d\n", hue_thr_from, hue_thr_to);
}
+ do_show = show_rgb || show_proc;
- f = fopen("pdfmask.bin", "rb");
- pdfmask_load_all(f, pdf_green);
- //pdfmask_load_all(f, pdf_red);
+ while (((f = fopen("pdfmask.bin", "rb")) == NULL) ||
+ pdfmask_load_all(f, pdf_green) ||
+ pdfmask_load_all(f, pdf_red)) {
+ perror("pdfmask.bin: fopen/pdfmask_load_all");
+ usleep(250*1000);
+ }
fclose(f);
- camera_on("/dev/video0", 640, 480, V4L2_PIX_FMT_MJPEG);
-
- for (i = 0;;) {
- camera_get_frame(process_jpeg_frame, 0);
- image_hue(rgb, hue, 320*480);
-
- if (do_grab || do_hue) {
- if (i++ > 15) {
- if (do_grab) {
- printf("P6\n640 480\n255\n");
- fwrite(rgb, 640*480*3, 1, stdout);
- }
- if (do_hue) {
- printf("P5\n320 480\n255\n");
- fwrite(hue, 320*480, 1, stdout);
- }
- return 0;
- }
- }
- else {
- image_threshold(hue, bitmap, 320*480, HUE_THR_LOW, HUE_THR_HIGH);
- lot = rozpuk(pdf_green);
- printf("%2d\n", 1+lot);
- }
+ while (orte_init() && !do_show) {
+ perror("orte_init");
+ usleep(500*1000);
}
+ if (do_show)
+ show();
+ else
+ rozpuk();
+
return 0;
}
ORTEPublicationSend(orte->publication_actuator_status);
break;
/* laser data */
- case CAN_CMU:
- orte->cmu.color = frame.data[0];
- ORTEPublicationSend(orte->publication_cmu);
+ case CAN_PUCK:
+ orte->puck_inside.status = frame.data[0];
+ ORTEPublicationSend(orte->publication_puck_inside);
break;
default:
robottype_publisher_motion_status_create(&orte, NULL, NULL);
robottype_publisher_motion_irc_create(&orte, NULL, NULL);
robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
- robottype_publisher_cmu_create(&orte, NULL, NULL);
robottype_publisher_corr_trig_create(&orte, NULL, NULL);
robottype_publisher_corr_distances_create(&orte, NULL, NULL);
robottype_publisher_puck_distance_create(&orte, NULL, NULL);
robottype_publisher_actuator_status_create(&orte, NULL, NULL);
+ robottype_publisher_puck_inside_create(&orte, NULL, NULL);
printf("Publishers OK\n");
/* creating subscribers */
/* spodni 3 bity: zapnout. dalsi 3 b zapnout */
#define CAN_PWR_ADC1 to_boa(0x41) /* PER->BOA */
-
#define CAN_PWR_ADC2 to_boa(0x42) /* PER->BOA */
/* napeti na jednotlivych vetvich, 4B hodnoty */
//#define CAN_BRUSHES_STATUS to_boa(0x44) // FIXME: (F.J.) rename me, ...
+#define CAN_PUCK to_boa(0x43)
+
+
#define CAN_CMU to_boa(0x46) /* PER->BOA */
#define CAN_HOKUYO_PITCH to_per(0x47) /* BOA->PER */
//#define CAN_ERROR to_boa(0x48) // FIXME: (F.J.) rename me, ...
+
+
#define CAN_LP_CMD to_per(0x80) // lift + pusher command
#define CAN_LP_STATUS to_boa(0x49) // Lift + Pusher Status 2B pusher position + 2B lift position + 1B status
#define CAN_LIFT to_per(0x4A) /**< BOA->PER @copydetails set_lift()*/
eb_sberac_09_SOURCES = main.c
eb_sberac_09_LIBS = can ebb uart_nozen
+link_VARIANTS = flash
+
-#define START_TIME 50
+#define START_TIME 16
deb_led_off(LEDY);
deb_led_off(LEDB);
deb_led_off(LEDG);
- deb_led_on(LEDR);
+// deb_led_on(LEDR);
PCON = 2; // bye bye
}
switch (rx_msg.id) {
case CAN_CHELAE:
deb_led_on(LEDB);
- time_timeout = time_ms + TIMEOUT_DELAY;
- deb_led_off(LEDY);
-
+ time_timeout = time_ms + TIMEOUT_DELAY;
set_servo(CHELAE_L , CHELAE_L_MIN + rx_msg.data[0] * (CHELAE_L_MAX - CHELAE_L_MIN)/255 );
set_servo(CHELAE_R , CHELAE_R_MIN + (0xFF - rx_msg.data[1]) * (CHELAE_R_MAX - CHELAE_R_MIN)/255 );
deb_led_on(LEDB);
time_timeout = time_ms + TIMEOUT_DELAY;
- deb_led_off(LEDY);
set_pwm(PWM_CHAN_B, rx_msg.data[0]);
set_pwm(PWM_CHAN_A, rx_msg.data[1]);
break;
void timeout(void)
{
- set_servo(CHELAE_L , CHELAE_L_MIN) ;
- set_servo(CHELAE_R , CHELAE_R_MAX) ;
- set_servo(HOKUYO , HOKUYO_AHEAD) ;
+// set_servo(CHELAE_L , CHELAE_L_MIN) ;
+// set_servo(CHELAE_R , CHELAE_R_MAX) ;
+// set_servo(HOKUYO , HOKUYO_AHEAD) ;
engine_A_pwm(0);
engine_B_pwm(0);
}
+#define PUCK_PIN 18 // MISO1
+uint32_t time_puck = 0;
+#define PUCK_TIME 50
+
+void puck_switch(void)
+{
+ if (time_puck == 0) time_puck= time_ms + PUCK_TIME;
+
+ if (time_ms > time_puck)
+ {
+ time_puck = time_ms + PUCK_TIME;
+
+ can_msg_t msg_puck;
+ msg_puck.id = CAN_PUCK;
+ msg_puck.dlc = 1;
+
+ if(IO0PIN & (1<< PUCK_PIN))
+ {
+ msg_puck.data[0] = 0;
+ deb_led_off(LEDR);
+ }
+ else
+ {
+ msg_puck.data[0] = 1;
+ deb_led_on(LEDR);
+ }
+
+ while (can_tx_msg(&msg_puck));
+
+ }
+}
+
+
uint8_t start_bt = 0;
uint32_t time_start = 0;
+uint8_t start_cnt = 0;
#define START_PIN 15 // start pin
void start_button(void)
{
if (start_bt > 10)
{
- if ((IO0PIN & (1<<START_PIN))==0) start_bt=0;
+ if ((IO0PIN & (1<<START_PIN))==0)
+ {
+ start_bt = 0;
+ start_cnt = 0;
+ }
return;
}
+
if (time_start == 0) time_start = time_ms + START_TIME;
if (IO0PIN & (1<<START_PIN))
{
- ++start_bt;
- msg.data[0] = 1;
- deb_led_off(LEDY);
+ ++start_cnt;
+ if(start_cnt > 3)
+ {
+ ++start_bt;
+ msg.data[0] = 1;
+ deb_led_off(LEDY);
+ }
}
else
{
deb_led_on(LEDY);
msg.data[0] = 0;
+ start_cnt = 0;
}
msg.id = CAN_ROBOT_CMD;
msg.flags = 0;
msg.dlc = 1;
- //msg.data[0] = (~(IO0PIN >> START_PIN)) & 1;
while (can_tx_msg(&msg));
}
}
SET_PIN(PINSEL0, START_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, (PUCK_PIN - 15), PINSEL_0);
while(1)
{
-// if( time_ms > time_timeout)
-// {
-// timeout();
-// deb_led_on(LEDY);
-// }
+ if( time_ms > time_timeout)
+ {
+ timeout();
+ //deb_led_on(LEDY);
+ }
start_button();
led_blik(LEDG);
send_adc();
+ puck_switch();
if(adc_update_adc & 1) adc_filter(0);
if(adc_update_adc & 2) adc_filter(1);
#define LIFT_SHIFT 0x1C
/* Positions of lift */
+#define LIFT_MIN 0x0
#define LIFT_GROUND_POS 0x0028 // used for pucks loading
-#define LIFT_TRAVEL_POS 0x0040
+#define LIFT_TRAVEL_POS 0x0030
#define LIFT_BELOW_HOLDER_POS 0x00E0
#define LIFT_IN_HOLDER_POS 0x0118
#define LIFT_FLOOR0 0x0030 // used for unloading
BASIC_LIFT_POSITION basic_lift_position = HIGH;
int pucks_loaded = 0;
bool lintel_present = 1;
-unsigned char floor_to_unload;
+int floor_to_unload;
/************************************************************
* STATE DECLARATIONS
{
switch (FSM_EVENT) {
case EV_ENTRY:
- printf("wait_for_command_state_entered\n");
+ printf("wait_for_command_state_entered, number of pucks stored: %d\n", pucks_loaded);
// put all actuators to defined initial positions
act_belts(BELTS_OFF, BELTS_OFF);
if (pucks_loaded<4) {
act_lift(basic_lift_position);
switch(basic_lift_position) {
case (LOW):
- printf("basic lift position set to LOW");
+ printf("basic lift position set to LOW\n");
break;
case (HIGH):
- printf("basic lift position set to HIGH");
+ printf("basic lift position set to HIGH\n");
break;
}
act_pusher(PUSHER_INSIDE);
act_hokuyo(HOKUYO_PITCH_HORIZONTAL);
break;
case EV_PREPARE_FOR_LOAD:
- basic_lift_position = HIGH;
- FSM_TRANSITION(wait_for_command);
+ //basic_lift_position = LOW;
+ //FSM_TRANSITION(wait_for_command);
break;
case EV_SCRABBLE:
FSM_TRANSITION(scrabble);
static int timer_arrival_count;
switch (FSM_EVENT) {
case EV_ENTRY:
+ act_lift(LIFT_MIN);
+ case EV_LIFT_IN_POS:
timer_arrival_count = 0;
printf("suck_the_puck state entered\n");
- FSM_TIMER(2000);
+ FSM_TIMER(800);
+ //FSM_TIMER(2000);
act_chelae(CHELA_TIGHT, CHELA_TIGHT);
act_belts(BELTS_IN, BELTS_IN);
break;
case EV_PUCK_INSIDE:
+ case EV_TIMER:
printf("puck inside\n");
- act_belts(BELTS_OFF, BELTS_OFF);
- act_chelae(CHELA_OPEN, CHELA_OPEN);
- act_holder(HOLDER_OPENED);
FSM_TRANSITION(store_pucks_in_holder);
break;
- case EV_TIMER:
+ /*case EV_TIMER:
switch (timer_arrival_count) {
case 0:
act_belts(BELTS_OUT, BELTS_OUT);
FSM_TRANSITION(wait_for_command);
break;
}
- timer_arrival_count++;
+ timer_arrival_count++; */
break;
case EV_RETURN:
case EV_LOAD_THE_PUCK:
- case EV_LIFT_IN_POS:
case EV_PUSHER_IN_POS:
case EV_PREPARE_THE_UNLOAD:
case EV_UNLOAD_PUCKS:
static int i;
switch (FSM_EVENT) {
case EV_ENTRY:
+ act_holder(HOLDER_OPENED);
if (pucks_loaded > 2) {
pucks_loaded++; // FIXME: (?) change this variable only here in this state?
+ basic_lift_position = HIGH;
FSM_TRANSITION(wait_for_command);
FSM_SIGNAL(MAIN, EV_ACTION_DONE, (void*)pucks_loaded);
} else {
break;
case 1:
pucks_loaded++; // FIXME: (?) change this variable in EV_ENTRY and only in one place?
+ basic_lift_position = HIGH;
FSM_TRANSITION(wait_for_command);
FSM_SIGNAL(MAIN, EV_ACTION_DONE, (void*)pucks_loaded);
}
{
switch (FSM_EVENT) {
case EV_ENTRY:
+ act_chelae(CHELA_OPEN, CHELA_OPEN);
printf("unloading pucks\n");
if (pucks_loaded < 4) {
act_lift(LIFT_BELOW_HOLDER_POS);
void robot_destroy()
{
motion_control_done();
+
+ act_chelae(CHELA_OPEN, CHELA_OPEN);
+ act_holder(HOLDER_OPENED);
robottype_roboorte_destroy(&robot.orte);
#define __robot_lock_disp lock_disp
#define __robot_lock_motion_irc lock
#define __robot_lock_corr_distances lock
+#define __robot_lock_camera_result lock_camera
/* robot's main data structure */
struct robot {
pthread_mutex_t lock_meas_angles;
pthread_mutex_t lock_joy_data;
pthread_mutex_t lock_disp;
+ pthread_mutex_t lock_camera;
/* competition parameters */
enum team_color team_color;
double puck_distance; /* sharp sensor to puck distance in meters */
struct hokuyo_scan_type hokuyo;
- struct cmu_type cmu;
struct map *map; /* Map for pathplanning (no locking) */
break;
}
}
+
+void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA: {
+ ROBOT_LOCK(camera_result);
+ robot.game_conf = instance->lot;
+ ROBOT_UNLOCK(camera_result);
+ break;
+ }
+ case DEADLINE:
+ DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+ break;
+ }
+}
/* ----------------------------------------------------------------------
* SUBSCRIBER CALLBACKS - EB2008
* ---------------------------------------------------------------------- */
}
}
-#if 0
-void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
- struct cmu_type *instance = (struct cmu_type *)vinstance;
- static enum ball_color last_color = NO_BALL;
- static unsigned char first = 1;
-
+ static bool previous_switch_status;
switch (info->status) {
- case NEW_DATA: {
- ROBOT_LOCK(cmu);
- robot.cmu = *instance;
- ROBOT_UNLOCK(cmu);
- if (first) {
- last_color = robot.cmu.color;
- first = 0;
- }
- if (robot.cmu.color != NO_BALL) {
- if (last_color != robot.cmu.color) {
- last_color = robot.cmu.color;
- //FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
- }
- }
- robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
- break;
+ case NEW_DATA: {
+ if (robot.orte.puck_inside.status == 1 && previous_switch_status != 1)
+ FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+ previous_switch_status = robot.orte.puck_inside.status;
}
- case DEADLINE:
- robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
- DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
- break;
+ break;
+ case DEADLINE:
+ DBG("ORTE deadline occurred - servos receive\n");
+ break;
}
}
-#endif
#define HIST_CNT 5
#if 0
robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
/* create generic subscribers */
robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_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_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 subscribers */
robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
- robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
- //robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
+ //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
+ robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte);
+ robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
return rv;
}
case 'p' :
case 'P' :
printf("Sending 'EV_PREPARE_THE_UNLOAD' event to the act fsm\n");
- FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, NULL);
+ FSM_SIGNAL(ACT, EV_PREPARE_THE_UNLOAD, (void*) floor);
break;
case 'u' :
case 'U' :
}
fflush(stdout);
}
- robot_destroy();
return NULL;
}
int main()
{
-
terminal_setup(); // disable canonical end echo modes of the terminal
pthread_attr_t tattr;
robot.fsm.main.debug_states = 0;
robot.fsm.motion.debug_states = 0;
- robot.fsm.act.debug_states = 0;
+ robot.fsm.act.debug_states = 1;
robot.fsm.main.state = &fsm_state_main_just_receive_events;
rv = robot_start();
if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+ robot_destroy();
terminal_cleanup(); // reset terminal mode to the original state
octet lot;
};
+struct puck_inside {
+ octet status;
+};
type=pwr_voltage topic=pwr_voltage
type=ref_pos topic=ref_pos
type=robot_cmd topic=robot_cmd
+type=camera_result topic=camera_result
+type=camera_control topic=camera_control
+type=puck_inside topic=puck_inside
type=fsm_state topic=fsm_main pubdelay=1
type=fsm_state topic=fsm_act pubdelay=1
type=fsm_state topic=fsm_motion pubdelay=1
projects / eurobot / public.git / commitdiff