Projekte
BirgerT
nibo2_fork#01
nibo2_copro.c
Projekte
Forum
Doku
Öffentliche Projekte
Startseite
Beispielprogramme
Projekte von anderen
Welcome
Username
Passwort
Eingeloggt bleiben
Zugangsdaten vergessen?
Registrieren
Projektverwaltung
⇨ Please choose! ⇦
——————————————————
✎ Create new project...
★ Browse existing projects...
——————————————————
⚬ MotorTest#1
⚬ C Tutorial 8#1
⚬ NIBO2 C Project#1
⚙ C Tutorial 15#1
⚬ 2010_11_18_el_test001#1
nibo2_fork#01
BSD_license.h
bt_lib_debugutil.c
bt_lib_debugutil.h
bt_lib_glcd.c
bt_lib_glcd.h
bt_lib_grafix.c
bt_lib_grafix.h
bt_lib_terminal.c
bt_lib_terminal.h
bt_lib_uart.c
bt_lib_uart.h
bt_lib_utils.h
lumpylumpy.h
main.c
nibo2_audio.c
nibo2_audio.h
nibo2_copro.c
nibo2_copro.h
nibo2_copro_cmd.h
nibo2_font.h
nibo2_global.h
nibo2_main_menu.h
nibo2_pwm_leds.c
nibo2_pwm_leds.h
nibo2_register.c
nibo2_register.h
nibo2_sensors.c
nibo2_sensors.h
test_debugutil.h
test_gfx_draw.h
xmas1.h
Project details
Compiler settings
Nachrichten
Sie sind nicht eingeloggt.
Neuigkeiten
★
NiboRoboLib 3.6
2017-01-17: Neue Version 3.6
★
NiboRoboLib 3.4.1
2016-04-16: Neue Version 3.4.1
★
Coding Tutorial
2015-11-22: Jetzt auch für den NIBO burger!
Site-Statistic
7426 private projects
385 public projects
16180353 lines compiled
58212 builds
NIBO
@
nibo2_fork_01/nibo2_copro.c [read only]
////////////////////////////////////////////////////////////////////////// // // NIBO2 COPRO KOMMUNIKATION über SPI // // Beschreibung siehe nibo2_pwm_leds.h // // Die Kommunikation entspricht der originalen Bibliotheksfunktion für // den CoPro mit der Initializer2 Firmware (c) nicai-Systems // ////////////////////////////////////////////////////////////////////////// // // BSD License see "license.h" #include "BSD_license.h" // #include "nibo2_copro.h" // Nibo2 Global Header // /* ******************************************* GLOBALE VARIABLEN Diese Variablen stehen dem Anwender in seinem Programm als Prozessabbild zur Verfügung. Die Aktualisierung von den Sensoren und die Ausgabe an die Aktoren erfolgt zyklisch immer vor dem Aufruf von der Funktionen startup_SW3(), startup_loop(), main_setup(), main_loop(), */ int8_t speed_l; int8_t speed_r; int16_t ticks_l; int16_t ticks_r; int16_t offset_l; int16_t offset_r; int16_t current_l; int16_t current_r; uint16_t distance; uint8_t ir_mode; uint8_t ir_distance[5]; uint8_t ir_count; uint16_t ir_cmd; // Bitmasken Sensorabstände uint8_t ir_sensors[3]; uint8_t ir_sensors_F[3]; uint8_t ir_sensors_S[3]; uint8_t spi_frame_cnt; /* ******************************************* LOKALE VARIABLEN und Konstanten, auf die nur die Funktionen in plc_main Zugriff haben sollen, und deshalb nicht in globaldef.h deklariert werden. */ uint8_t spi_rx_buf[SPI_BUF_SIZE]; uint8_t spi_tx_buf[SPI_BUF_SIZE]; uint8_t spi_size; uint8_t spi_pos; uint8_t spi_tx_chk; uint8_t spi_rx_chk; uint8_t spi_err_cnt; uint8_t spi_rx_size; uint8_t spi_tx_size; uint8_t copro_buf[SPI_BUF_SIZE]; uint8_t copro_cmds[3][7]; uint8_t copro_seq; /* ********************************************* MAIN FUNCTIONS Funktionen zur */ uint8_t spi_callback(uint8_t rx_size) { uint8_t pos = 0; copro_seq++; if (rx_size == SPI_BUF_SIZE) { memcpy(copro_buf,spi_rx_buf,SPI_BUF_SIZE); } spi_tx_buf[0]=spi_tx_buf[7]=spi_tx_buf[14] = 0xff; if (copro_cmds[0][0] != 0xff) { memcpy(spi_tx_buf + pos, copro_cmds[0], 7); copro_cmds[0][0] = 0xff; pos += 7; } if (copro_cmds[1][0] != 0xff) { memcpy(spi_tx_buf + pos, copro_cmds[1], 7); copro_cmds[1][0] = 0xff; pos += 7; } if (copro_cmds[2][0] != 0xff) { memcpy(spi_tx_buf + pos, copro_cmds[2], 7); copro_cmds[2][0] = 0xff; pos += 7; } return pos?21:7; } uint8_t _copro_busy(uint8_t ch) { return (volatile uint8_t)(copro_cmds[ch][0]) != 0xff; } static uint8_t _copro_wait(uint8_t ch) { sei(); uint16_t cnt = 10000; while((--cnt) && _copro_busy(ch)){} return cnt != 0; } void copro_update() { cli(); ir_distance[0] = copro_buf[1]; ir_distance[1] = copro_buf[3]; ir_distance[2] = copro_buf[5]; ir_distance[3] = copro_buf[7]; ir_distance[4] = copro_buf[9]; sei(); cli(); ticks_l = TOWORD(copro_buf[11],copro_buf[10]) + offset_l; ticks_r = TOWORD(copro_buf[13],copro_buf[12]) + offset_r; speed_l = copro_buf[14]; speed_r = copro_buf[16]; current_l = TOWORD(copro_buf[19],copro_buf[18]); current_r = TOWORD(copro_buf[21],copro_buf[20]); sei(); cli(); ir_cmd = TOWORD(copro_buf[24],copro_buf[23]); distance = TOWORD(copro_buf[26],copro_buf[25]); sei(); } void copro_stop() { cli(); copro_cmds[0][0] = COPRO_CMD_STOP; sei(); } void copro_setPWM(int16_t left, int16_t right) { cli(); copro_cmds[0][0] = COPRO_CMD_SETPWM; copro_cmds[0][1] = LOBYTE(left); copro_cmds[0][2] = HIBYTE(left); copro_cmds[0][3] = LOBYTE(right); copro_cmds[0][4] = HIBYTE(right); sei(); } void copro_setSpeed(int8_t left, int8_t right) { cli(); copro_cmds[0][0] = COPRO_CMD_SETSPEED; copro_cmds[0][1] = LOBYTE(left); copro_cmds[0][2] = 0; //HIBYTE(left); copro_cmds[0][3] = LOBYTE(right); copro_cmds[0][4] = 0; // HIBYTE(right); sei(); } void copro_setTargetAbs(int16_t left, int16_t right, int8_t speed) { left -= offset_l; right -= offset_r; cli(); copro_cmds[0][0] = COPRO_CMD_SETTARGET_ABS; copro_cmds[0][1] = LOBYTE(left); copro_cmds[0][2] = HIBYTE(left); copro_cmds[0][3] = LOBYTE(right); copro_cmds[0][4] = HIBYTE(right); copro_cmds[0][5] = speed; copro_cmds[0][6] = 0; sei(); } void copro_setTargetRel(int16_t left, int16_t right, int8_t speed) { cli(); copro_cmds[0][0] = COPRO_CMD_SETTARGET_REL; copro_cmds[0][1] = LOBYTE(left); copro_cmds[0][2] = HIBYTE(left); copro_cmds[0][3] = LOBYTE(right); copro_cmds[0][4] = HIBYTE(right); copro_cmds[0][5] = speed; copro_cmds[0][6] = 0; sei(); } void copro_setPID(int8_t ki, int8_t kp, int8_t kd) { _copro_wait(2); cli(); copro_cmds[2][0] = COPRO_CMD_SETPARAMETERS; copro_cmds[2][1] = kp; copro_cmds[2][2] = ki; copro_cmds[2][3] = kd; sei(); } void copro_setOdometer(int16_t left, int16_t right) { cli(); ticks_l = TOWORD(copro_buf[11], copro_buf[10]); ticks_r = TOWORD(copro_buf[13], copro_buf[12]); offset_l = left - ticks_l; offset_r = right - ticks_r; ticks_l = left; ticks_r = right; sei(); } void copro_ir_stop() { _copro_wait(1); cli(); copro_cmds[1][0] = COPRO_CMD_STOP_IR; sei(); } void copro_ir_start() { _copro_wait(1); cli(); copro_cmds[1][0] = COPRO_CMD_START_IR; sei(); } void copro_ir_send(uint16_t code) { _copro_wait(1); cli(); copro_cmds[1][0] = COPRO_CMD_SEND_RC5; copro_cmds[1][1] = LOBYTE(code); copro_cmds[1][2] = HIBYTE(code); sei(); } /* Definition der Grenzwerte in globalee.h // Grenzwerte für Distanzsensoren #define DIST_LIMIT_FREE 0x20 // freie Bahn #define DIST_LIMIT_FAR 0x40 // > Hindernis in Sicht #define DIST_LIMIT_NEAR 0x80 // > Hindernis ist nah //#define DIST_LIMIT_LOW 0xb0 // > Hindernis ausweichen */ void ir_sensors_update() { copro_update(); uint8_t mask = 0x10; uint8_t indx = 4; ir_sensors_F[0] = ir_sensors[0]; ir_sensors_F[1] = ir_sensors[1]; ir_sensors_F[2] = ir_sensors[2]; ir_sensors_S[0] = ~ir_sensors[0]; ir_sensors_S[1] = ~ir_sensors[1]; ir_sensors_S[2] = ~ir_sensors[2]; ir_sensors[0] = 0; ir_sensors[1] = 0; ir_sensors[2] = 0; do { if (ir_distance[indx] > eep_param.dist_low) { ir_sensors[0] |= mask; } else if (ir_distance[indx] > eep_param.dist_near) { ir_sensors[1] |= mask; } else if (ir_distance[indx] > eep_param.dist_free) { ir_sensors[2] |= mask; } mask = mask/2; } while (indx-- > 0); ir_sensors_S[0] &= ir_sensors[0]; ir_sensors_S[1] &= ir_sensors[1]; ir_sensors_S[2] &= ir_sensors[2]; ir_sensors_F[0] &= ~ir_sensors[0]; ir_sensors_F[1] &= ~ir_sensors[1]; ir_sensors_F[2] &= ~ir_sensors[2]; } void ir_sensors_show() { uint8_t imask = 0x01; uint8_t omask = 0x80; uint8_t gled = LED_GN & 0x03; uint8_t rled = LED_RD & 0x03; uint8_t i = 6; do { if (((ir_sensors[0] & imask) !=0) || ((ir_sensors[1] & imask) != 0)) { rled |= omask; } if (((ir_sensors[2] & imask) !=0) || ((ir_sensors[1] & imask) != 0)) { gled |= omask; } omask = omask >> 1; if (i !=4) { imask = imask << 1; } } while (i-- > 0); LED_RD = rled; LED_GN = gled; } static inline void spi_tx(uint8_t val) { SPDR = val; spi_tx_chk ^= val; } static inline uint8_t spi_rx(){ uint8_t val = SPDR; spi_rx_chk ^= val; return val; } uint8_t spi_restart(uint8_t size) { spi_pos = 0; spi_tx_size = size; spi_size = size +1; spi_tx_chk = 0x96; spi_rx_chk = 0x96; cli(); // while(GetBit(PINB,ISP_SS) == 0){ while((PINB & (1<<ISP_SS)) == 0){ sei(); NOP; cli(); } // SPI ist bereit, Register setzen SPCR = (1<<SPIE) | (1<<SPE); spi_tx(size); SPSR |= (1<<SPIF); sei(); return 0; } ISR(SPI_STC_vect){ // empfangen uint8_t rx_data = spi_rx(); if (spi_pos == 0) { spi_rx_size = rx_data; if (spi_rx_size > sizeof(spi_rx_buf)) { spi_rx_size = 0; spi_tx_size = 0; spi_size = 1; } else if (spi_rx_size +1 > spi_size) { spi_size = spi_rx_size +1; } } else { if (spi_pos -1 < spi_rx_size) { spi_rx_buf[spi_pos-1]= rx_data; } else if (spi_pos -1 == spi_rx_size) { if (spi_rx_chk != 0) { spi_err_cnt++; spi_rx_size = 0; } } } // senden if (spi_pos < spi_tx_size) { spi_tx(spi_tx_buf[spi_pos]); spi_pos++; } else if (spi_pos == spi_tx_size) { spi_tx(spi_tx_chk); spi_pos++; } else if (spi_pos < spi_size) { spi_tx(0xff); spi_pos++; } else { // disable IRQ CLRBIT(DDRB,ISP_SS); CLRBIT(PORTB,ISP_SS); SPCR = 0; // (1<<SPE); CLRBIT(DDRB,ISP_SS); CLRBIT(PORTB,ISP_SS); spi_frame_cnt++; sei(); spi_restart(spi_callback(spi_rx_size)); } } void copro_init() { // *********************************************************** COPRO INIT CO_RST_O; // Output CO_RST_L; // Reset = 0 // SPI Kommunikation CoPro Reset CLRBIT(DDRB,ISP_SS); // Input CLRBIT(PORTB,ISP_SS); // Pullup aus SETBIT(DDRB,ISP_MISO); // Output SETBIT(PORTB,ISP_MISO); // // CoPro Reset Ende CO_RST_H; // Reset = 1 spi_restart(0); // in nibo2_copro.c }
Compiler results:
Werbung
Online
leonelb389687792836
nelsonleeson85120051
ruebenmendes25697
warrenhinder6456
wilfredocastillo