#define CAN_SPEED 1000000 //< CAN bus speed
#define CAN_ISR 0
+#define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+#define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
#define ADC_ISR 1
-#define CL_SENSOR_IN 16
-#define CL_SENSOR_OUT EXPPORT_3
+#define CL_SENSOR_IN 16 //port for sending command to color sensor(via power switch)
#define JAW_LEFT 0
#define JAW_RIGHT 1
#define TIMER_IRQ_PRIORITY 5
-#define BUMPER_LEFT EXPPORT_5
-#define BUMPER_RIGHT EXPPORT_4
+#define BUMPER_LEFT GPIOPORT_8
+#define BUMPER_RIGHT GPIOPORT_6
-#define BUMPER_LEFT_ACROSS EXPPORT_6
-#define BUMPER_RIGHT_ACROSS EXPPORT_8
+#define BUMPER_LEFT_ACROSS GPIOPORT_3
+#define BUMPER_RIGHT_ACROSS GPIOPORT_1
-#define BUMPER_REAR_LEFT EXPPORT_7
-#define BUMPER_REAR_RIGHT EXPPORT_1
+#define CL_SENSOR_OUT/*BUMPER_REAR_LEFT*/ GPIOPORT_7 //port for receiving data from color sensor used to be BUMPER_REAR_LEFT
+#define BUMPER_REAR GPIOPORT_4
-#define COLOR_PIN GPIOPORT_1
-#define SWITCH_STRATEGY_PIN GPIOPORT_2
+#define COLOR_PIN0 EXPPORT_4
+#define COLOR_PIN1 EXPPORT_5
+#define SWITCH_STRATEGY_PIN EXPPORT_8
+#define START_PIN EXPPORT_7
struct fsm fsm_jaw_right;
VICIntEnable = 1<<source;
}
+void start_button(void)
+{
+ can_msg_t msg;
+ bool start_condition;
+ static bool last_start_condition = 0;
+
+ static int count = 0;
+ static uint32_t next_send = 0;
+
+
+ start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+
+ if (start_condition != last_start_condition) {
+
+
+ last_start_condition = start_condition;
+ count = 0;
+ next_send = timer_msec; /* Send now */
+
+ }
+
+ if (timer_msec >= next_send) {
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_condition;
+
+// send_rs_str("start\n");
+
+ /*while*/ (can_tx_msg(&msg));
+
+ if (count < START_SEND_FAST_COUNT) {
+ count++;
+ next_send = timer_msec + START_SEND_PRIOD_FAST;
+ } else
+ next_send = timer_msec + START_SEND_PRIOD_SLOW;
+ }
+
+
+}
+
+
void robot_switches_handler()
{
static uint32_t color_time = 0;
color_time = timer_msec;
- if (IO1PIN & (1<<COLOR_PIN))
- sw |= CAN_SWITCH_COLOR;
+ if (IO0PIN & (1<<COLOR_PIN0))
+ sw |= CAN_SWITCH_COLOR_0;
+ else
+ sw &= ~CAN_SWITCH_COLOR_0;
+
+ if (IO0PIN & (1<<COLOR_PIN1))
+ sw |= CAN_SWITCH_COLOR_1;
else
- sw &= ~CAN_SWITCH_COLOR;
+ sw &= ~CAN_SWITCH_COLOR_1;
- if (IO1PIN & (1<<SWITCH_STRATEGY_PIN))
+ if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
sw &= ~CAN_SWITCH_STRATEGY;
else
sw |= CAN_SWITCH_STRATEGY;
- if (sw & CAN_SWITCH_COLOR){
+ if ((sw & CAN_SWITCH_COLOR_0) || (sw & CAN_SWITCH_COLOR_1)){
deb_led_off(LEDY);
send_rs_str("color\n");}
tx_msg.dlc = 1;
tx_msg.flags = 0;
tx_msg.data[0] = sw;
-
can_tx_msg(&tx_msg);
}
}
}
+void cl_sensor_send_status(){
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_CL_SENSOR_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = ((IO1PIN & 1<<CL_SENSOR_OUT) != 0) & 0xFF;// 1 - pattern match 0 - no match
+ if((IO1PIN & 1<<CL_SENSOR_OUT) != 0) send_rs_str("trefa\n");
+ (can_tx_msg(&tx_msg));
+}
+
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- if (IO0PIN & (1<<BUMPER_REAR_LEFT)){
+ /*if (IO1PIN & (1<<BUMPER_REAR_LEFT)){
sw &= ~CAN_BUMPER_REAR_LEFT;
send_rs_str("rear_left\n");
sw |= CAN_BUMPER_REAR_LEFT;
// deb_led_off(LEDY);
- }
+ }*/
- if (IO0PIN & (1<<BUMPER_REAR_RIGHT)){
+ if (IO1PIN & (1<<BUMPER_REAR)){
sw &= ~CAN_BUMPER_REAR_RIGHT;
-
- send_rs_str("rear_right\n");
+ sw &= ~CAN_BUMPER_REAR_LEFT;
+ send_rs_str("reart\n");
// deb_led_on(LEDY);
}
else{
sw |= CAN_BUMPER_REAR_RIGHT;
+ sw |= CAN_BUMPER_REAR_LEFT;
// deb_led_off(LEDY);
}
- if (IO0PIN & (1<<BUMPER_LEFT)){
+ if (IO1PIN & (1<<BUMPER_LEFT)){
sw &= ~CAN_BUMPER_LEFT;
// deb_led_off(LEDB);
}
- if (IO0PIN & (1<<BUMPER_RIGHT)){
+ if (IO1PIN & (1<<BUMPER_RIGHT)){
sw &= ~CAN_BUMPER_RIGHT;
send_rs_str("right\n");
// deb_led_on(LEDG);
// deb_led_off(LEDG);
}
- if (IO0PIN & (1<<BUMPER_LEFT_ACROSS)){
+ if (IO1PIN & (1<<BUMPER_LEFT_ACROSS)){
sw &= ~CAN_BUMPER_LEFT_ACROSS;
send_rs_str("left_across\n");
// deb_led_on(LEDR);
// deb_led_off(LEDR);
}
- if (IO0PIN & (1<<BUMPER_RIGHT_ACROSS)){
+ if (IO1PIN & (1<<BUMPER_RIGHT_ACROSS)){
sw &= ~CAN_BUMPER_RIGHT_ACROSS;
send_rs_str("right_across\n");
// deb_le0d_on(LEDY);
// send_rs_int(IO1PIN);
// send_rs_int(sw);
// send_rs_str("\n");
-//
-
tx_msg.id = CAN_ROBOT_BUMPERS;
tx_msg.dlc = 1;
tx_msg.flags = 0;
- tx_msg.data[0] = sw;
-
+ tx_msg.data[0] = sw;
can_tx_msg(&tx_msg);
}
}
void cl_sensor_init(){
IO0DIR |= 1<<CL_SENSOR_IN;
- IO0DIR &= ~(1<<CL_SENSOR_OUT);
+ IO1DIR &= ~(1<<CL_SENSOR_OUT);
IO0SET = (1<<CL_SENSOR_IN);
option = 3;
option_new = 0;
uint32_t status_time_left = timer_usec;
uint32_t status_time_right = timer_usec;
uint32_t status_time_sensor = timer_msec;
- bool feedback;
+ //bool feedback;
init_periphery();
- SET_PIN(PINSEL1, 1, PINSEL_0);
- SET_PIN(PINSEL1, 2, PINSEL_0);
- SET_PIN(PINSEL1, 3, PINSEL_0);
- SET_PIN(PINSEL0, BUMPER_REAR_LEFT, PINSEL_0);
- SET_PIN(PINSEL0, BUMPER_REAR_RIGHT, PINSEL_0);
- SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
-
-
-// IO0DIR &= ~((1<<BUMPER_REAR) | (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
- PINSEL2 &= ~(1 << 3) ;
- IO1DIR &= ~((1<<COLOR_PIN) | (1<<SWITCH_STRATEGY_PIN));
+ SET_PIN(PINSEL0, COLOR_PIN0, PINSEL_0);
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, SWITCH_STRATEGY_PIN-16, PINSEL_0);
+ SET_PIN(PINSEL1, COLOR_PIN1-16, PINSEL_0);
+
+ PINSEL2 &= ~(1 << 3) ; //setting P1.25:16 as GPIO pins -> GPIO PORT on eb_board
+ IO1DIR &= ~((1<<BUMPER_REAR) | /*(1<<BUMPER_REAR_LEFT) |*/ (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
+ IO0DIR &= ~((1<<COLOR_PIN0) | (1<<COLOR_PIN1) | (1<<START_PIN) | (1<<SWITCH_STRATEGY_PIN));
cl_sensor_init();
send_rs_str("Jaws started\n");
//uint32_t i = 5000000;
//letf jaw potenciometer on ADC 0, header J32 on board
fsm_jaw_left.max_pos=0x320; //max 0x324
fsm_jaw_left.min_pos=0xB0; //min 0xC3
- fsm_jaw_left.act_pos = adc_val[0];
+ //fsm_jaw_left.act_pos = adc_val[0];
//close jaw, max_pos, open jaw min_pos
//letf jaw potenciometer on ADC 1, header J33 on board
fsm_jaw_right.max_pos=0x380; //max 0x382
fsm_jaw_right.min_pos=0x010; //min 0xF5
- fsm_jaw_right.act_pos = adc_val[1];
+ //fsm_jaw_right.act_pos = adc_val[1];
//left jaw engine is engine A, conector MOTA on board
- fsm_jaw_left.engine_en = &engine_A_en;
- fsm_jaw_left.engine_dir = &engine_A_dir;
- fsm_jaw_left.engine_pwm = &engine_A_pwm;
+ //fsm_jaw_left.engine_en = &engine_A_en;
+ //fsm_jaw_left.engine_dir = &engine_A_dir;
+ //fsm_jaw_left.engine_pwm = &engine_A_pwm;
fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
//right jaw engine is engine B, conector MOTB on board
- fsm_jaw_right.engine_en = &engine_B_en;
- fsm_jaw_right.engine_dir = &engine_B_dir;
- fsm_jaw_right.engine_pwm = &engine_B_pwm;
+ //fsm_jaw_right.engine_en = &engine_B_en;
+ //fsm_jaw_right.engine_dir = &engine_B_dir;
+ //fsm_jaw_right.engine_pwm = &engine_B_pwm;
fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
+
init_fsm(&fsm_jaw_right, &fsm_jaw_init);
init_fsm(&fsm_jaw_left, &fsm_jaw_init);
//dbg_print_time();
//fsm_jaw_left.act_pos = adc_val[0];
- //run_fsm(&fsm_jaw_left);
+ run_fsm(&fsm_jaw_left);
//fsm_jaw_right.act_pos = adc_val[1];
- //run_fsm(&fsm_jaw_right);
+ run_fsm(&fsm_jaw_right);
}
if(timer_msec >= status_time_sensor + 10){
}
}
}
+ cl_sensor_send_status();
// feedback = (IO0PIN & 1<<CL_SENSOR_OUT) != 0;
// if(feedback) send_rs_str("shoda barev\n");
// else send_rs_str("jina barva\n");
fsm_jaw_left.trigger_can_send*/) { //or when something important happen
//fsm_jaw_left.trigger_can_send = false;
status_time_left = timer_msec; //save new time, when message was sent
- // can_send_status(&fsm_jaw_left);
+ can_send_status(&fsm_jaw_left);
}
fsm_jaw_right.trigger_can_send*/) { //or when something important happen
//fsm_jaw_right.trigger_can_send = false;
status_time_right = timer_msec; //save new time, when message was sent
- // can_send_status(&fsm_jaw_right);
+ can_send_status(&fsm_jaw_right);
- }
-
+ }
+ start_button();
handle_bumper();
robot_switches_handler();
blink_led();
projects / eurobot / public.git / commitdiff