--- /dev/null
+////////////////////////////////////////////////////////////////////////////////
+//
+// Eurobot BLINK TEST (with LPC2129)
+//
+// Description
+// -----------
+// This software blinks on debug leds. Use it with lpceurobot board
+// Author : Jiri Kubias DCE CVUT
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <errno.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include <can_ids.h>
+#include <can_msg_def.h>
+#include <servo.h>
+#include <powswitch.h>
+#include <engine.h>
+#include <adc_filtr.h>
+#include <uart_nozen.h>
+
+
+
+
+#define CAN_SPEED 1000000
+#define CAN_ISR 0
+#define UART0_ISR 1
+#define UART1_ISR 2
+#define SERVO_ISR 4
+#define ADC_ISR 5
+
+#define CR 0x0d
+
+#define PWM_CHAN_A 0
+#define PWM_CHAN_B 1
+
+#define ADC_TIME 60
+
+#define SERVO_0_BOUND 0x10 // min for servo S0
+#define SERVO_1_BOUND 0xFA // max for servo S1
+
+#define TIMEOUT_DELAY 3000
+
+
+
+
+/**
+ * @addtogroup leds
+ * @{
+ * @name Picker_09 (lpceurobot)
+ * @{
+ */
+#define LED_GREEN /**< Blinks once per second when software is alive */
+#define LED_BLUE /**< Blinks when a CAN message is received: */
+#define LED_YELLOW /**< Light: when timeout occurs */
+#define LED_RED /**< Not used */
+/** @} */
+/** @} */
+
+
+
+
+
+
+can_msg_t msg;
+
+unsigned int time_blink;
+volatile unsigned int time_timeout;
+volatile unsigned int sberac_timeout;
+volatile unsigned sberac_timeout_en = 0;
+
+
+void led_blik(unsigned char led)
+{
+ if (time_blink == 0) time_blink = time_ms + 500;
+
+ if (time_ms > time_blink)
+ {
+ deb_led_change(led);
+ time_blink = time_ms + 500;
+ }
+
+}
+
+
+void atoim(unsigned int val)
+{
+ write_UART_data( UART0, ' ' );
+ write_UART_data( UART0, (val / 1000 + 48) );
+ write_UART_data( UART0, ((val % 1000) / 100 + 48) );
+ write_UART_data( UART0, ((val % 1000 % 100) / 10 + 48) );
+ write_UART_data( UART0, ((val % 100) % 10 + 48) );
+
+}
+
+unsigned int time_adc =0;
+void send_adc()
+{
+ if (time_adc == 0) time_adc = time_ms + ADC_TIME;
+
+ if (time_ms > time_adc)
+ {
+ adc_filter(adc_update_adc);
+ unsigned int data[4];
+ data[0] = adc_val[0];
+ data[1] = adc_val[1];
+ data[2] = adc_val[2];
+ data[3] = adc_val[3];
+
+
+ atoim(data[0]);
+ atoim(data[1]);
+ atoim(data[2]);
+ atoim(data[3]);
+
+
+ msg.id = CAN_ADC_1;
+ msg.flags = 0;
+ msg.dlc = 8;
+ msg.data[0] = (data[0]>>8) & 0xFF;
+ msg.data[1] = data[0] & 0xFF;
+ msg.data[2] = (data[1]>>8) & 0xFF;
+ msg.data[3] = data[1] & 0xFF;
+ msg.data[4] = (data[2]>>8) & 0xFF;
+ msg.data[5] = data[2] & 0xFF;
+ msg.data[6] = (data[3]>>8) & 0xFF;
+ msg.data[7] = data[3] & 0xFF;
+ while (can_tx_msg(&msg));
+
+
+ time_adc = time_ms + ADC_TIME;
+ }
+
+}
+
+
+void set_pwm(unsigned char chan , unsigned char pwm)
+{
+ unsigned int dir = 0;
+ unsigned int speed = 0;
+
+ if ( pwm > 100 )
+ {
+ dir = 1;
+ speed = pwm - 100;
+ }
+ else
+ {
+ dir = 0;
+ speed = 100 - pwm;
+ }
+
+ if (chan == PWM_CHAN_A)
+ {
+ engine_A_pwm(speed);
+ if (dir)
+ engine_A_dir(ENGINE_DIR_FW);
+ else
+ engine_A_dir(ENGINE_DIR_BW);
+ }
+ else
+ {
+ engine_B_pwm(speed);
+ if (dir)
+ engine_B_dir(ENGINE_DIR_FW);
+ else
+ engine_B_dir(ENGINE_DIR_BW);
+ }
+}
+
+void go_to_sleep()
+{
+ engine_A_en(ENGINE_EN_OFF); // engines off
+ engine_A_pwm(0);
+ engine_B_en(ENGINE_EN_OFF);
+ engine_B_pwm(0);
+
+ can_off();
+
+ deb_led_off(LEDY);
+ deb_led_off(LEDB);
+ deb_led_off(LEDG);
+ deb_led_on(LEDR);
+ PCON = 2; // bye bye
+}
+
+
+void can_rx(can_msg_t *msg) {
+ can_msg_t rx_msg;
+
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));
+
+
+ switch (rx_msg.id) {
+ case CAN_SERVO:
+ deb_led_on(LEDB);
+ time_timeout = time_ms + TIMEOUT_DELAY;
+ deb_led_off(LEDY);
+
+ if(rx_msg.data[0] < SERVO_0_BOUND) rx_msg.data[0] = SERVO_0_BOUND;
+ if(rx_msg.data[1] > SERVO_1_BOUND) rx_msg.data[1] = SERVO_1_BOUND;
+
+ set_servo(0, rx_msg.data[0]);
+ set_servo(1, rx_msg.data[1]);
+ set_servo(2, rx_msg.data[2]);
+
+ break;
+
+ case CAN_DRIVES:
+
+ 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;
+
+ case CAN_PWR_ALERT:
+ if ( (rx_msg.data[0] == 2) | (rx_msg.data[0] == 3) ) go_to_sleep();
+ break;
+
+
+ case CAN_PICKER:
+ sberac_timeout_en = 1;
+ deb_led_on(LEDB);
+ deb_led_off(LEDY);
+ set_servo(0, rx_msg.data[0]);
+ set_servo(1, rx_msg.data[1]);
+ set_pwm(PWM_CHAN_B, rx_msg.data[2]);
+ set_pwm(PWM_CHAN_A, rx_msg.data[3]);
+ time_timeout = time_ms + TIMEOUT_DELAY;
+ sberac_timeout = time_ms + ((uint32_t)rx_msg.data[4])*100;
+ break;
+
+ default:
+ break;
+ }
+
+ deb_led_off(LEDB);
+}
+
+
+
+void check_sberac_timeout(void)
+{
+ if ((sberac_timeout < time_ms) & sberac_timeout_en)
+ {
+ set_servo(0, SERVO_0_BOUND);
+ set_servo(1, SERVO_1_BOUND);
+ //set_servo(2, 0x80);
+ engine_A_pwm(0);
+ engine_B_pwm(0);
+ sberac_timeout_en = 0;
+ }
+
+}
+
+void init_perip(void) // inicializace periferii mikroprocesoru
+{
+ init_servo(SERVO_ISR);
+ init_pow_switch();
+
+ init_engine_A();
+ init_engine_B();
+ engine_A_en(ENGINE_EN_OFF);
+ engine_B_en(ENGINE_EN_OFF);
+
+ init_adc_filter(ADC_ISR);
+
+ pow_switch_off();
+
+ UART_init( UART0,UART0_ISR,19200, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0);
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
+}
+
+
+ int time_delay =0;
+
+
+void timeout(void)
+{
+ set_servo(0, SERVO_0_BOUND);
+ set_servo(1, SERVO_1_BOUND);
+ set_servo(2, 0x80);
+ engine_A_pwm(0);
+ engine_B_pwm(0);
+}
+
+
+int main (void) {
+
+ init_perip(); // sys init MCU
+
+ set_servo(0, 0x50);
+ set_servo(1, 0xB0);
+ set_servo(2, 0x80);
+
+ time_delay = time_ms + TIMEOUT_DELAY;
+
+ deb_led_on(LEDY);
+ while(time_ms < time_delay) led_blik(LEDG);
+ deb_led_off(LEDY);
+
+ engine_A_en(ENGINE_EN_ON);
+ engine_A_dir(ENGINE_DIR_FW);
+ engine_A_pwm(0);
+ engine_B_en(ENGINE_EN_ON);
+ engine_B_dir(ENGINE_DIR_FW);
+ engine_B_pwm(0);
+ sberac_timeout_en = 0;
+
+
+
+ time_timeout = time_ms + TIMEOUT_DELAY;
+
+
+ while(1)
+ {
+
+ if( time_ms > time_timeout)
+ {
+ timeout();
+ deb_led_on(LEDY);
+ }
+ led_blik(LEDG);
+ check_sberac_timeout();
+ send_adc();
+
+ if(adc_update_adc & 1) adc_filter(0);
+ if(adc_update_adc & 2) adc_filter(1);
+ if(adc_update_adc & 4) adc_filter(2);
+ if(adc_update_adc & 8) adc_filter(3);
+
+
+ }
+}
+
+
projects / eurobot / public.git / commitdiff