00001 00040 /***************************************************************************** 00041 * * 00042 * it is not allowed to remove the nicknames of the contributers to this * 00043 * software from the function header * 00044 * * 00045 *****************************************************************************/ 00046 /***************************************************************************** 00047 * * 00048 * This program is free software; you can redistribute it and/or modify * 00049 * it under the terms of the GNU General Public License as published by * 00050 * the Free Software Foundation; either version 2 of the License, or * 00051 * any later version. * 00052 * * 00053 *****************************************************************************/ 00054 00055 00056 00057 /**************************************************************************** 00058 * 00059 * File Name: asuro.c 00060 * 00061 * 00062 * Ver. Date Author Comments 00063 * ------- ---------- -------------- ------------------------------ 00064 * 1.00 14.08.2003 Jan Grewe build 00065 * 2.00 14.10.2003 Jan Grewe RIGHT_VEL -> MotorSpeed(unsigned char left_speed, unsigned char right_speed); 00066 * LeftRwd(),LeftFwd(),RightRwd(), LEFT_VEL, 00067 * RigthFwd() -> MotorDir(unsigned char left_dir, unsigned char right_dir); 00068 * GREEN_ON,GREEN_OFF,RED_ON,RED_OFF -> StatusLED(unsigned char color); 00069 * LED_RED_ON, LED_RED_OFF -> FrontLED(unsigned char color); 00070 * Blink(unsigned char left, unsigned char right) -> 00071 * BackLED(unsigned char left, unsigned char right); 00072 * Alles in Funktionen gefasst => leichter verständlich ?!?! 00073 * 2.10 17.10.2003 Jan Grewe new Timer funktion void Sleep(unsigned char time36kHz) 00074 * 00075 * 00076 * Copyright (c) 2003 DLR Robotics & Mechatronics 00077 *****************************************************************************/ 00078 /**************************************************************************** 00079 * 00080 * File Name: asuro.c 00081 * Project : asuro library "Robotrixer Buxtehude" 00082 * 00083 * Description: This file contains additional functions: 00084 * 00085 * signal (SIG_ADC) interrupt/signal routine for encoder-counter 00086 * signal (SIG_INTERRUPT1) signal for switches 00087 * EncoderInit() initializing encoder-counter 00088 * EncoderStart() start autoencoding 00089 * EncoderStop() stop autoencoding 00090 * EncoderSet(int,int) set encodervalue 00091 * Msleep(int delay) wait for delay in milliseconds 00092 * Gettime() get systemtime in milliseconds 00093 * PrintInt(int) 00094 * 00095 * modifications in Sleep, SIG_OUTPUT_COMPARE2, PollSwitch, LineData 00096 * 00097 * Ver. Date Author Comments 00098 * ------- ---------- -------------- ------------------------------ 00099 * beta1 31.03.2005 Robotrixer asuro library 00100 * ------- ---------- -------------- ------------------------------ 00101 * the encoder source is based on RechteckDemo.c ver 2.0 by Jan Grewe 22.10.2003 00102 * Copyright (c) 2003 DLR Robotics & Mechatronics 00103 00104 *****************************************************************************/ 00105 /**************************************************************************** 00106 * 00107 * File Name: asuro.c 00108 * Project : asuro library modified for IR collision detector 00109 * 00110 * Description: modifications made in following functions: 00111 * 00112 * SIGNAL (SIG_OUTPUT_COMPARE2) -> SIGNAL (SIG_OVERFLOW2) 00113 * Gettime() counts now 36kHz 00114 * Init() timer2 modified for adjustable duty cycle 00115 * Batterie() bug fixed 00116 * Sleep() counts now 36kHz 00117 * Msleep() counts now 36kHz 00118 * 00119 * Ver. Date Author Comments 00120 * ------- ---------- -------------- ------------------------------ 00121 * beta2 11.06.2005 Waste asuro library 00122 * ------- ---------- -------------- ------------------------------ 00123 *****************************************************************************/ 00124 /**************************************************************************** 00125 * 00126 * File Name: asuro.c 00127 * Project : asuro library 00128 * 00129 * Description: This file contains additional functions: 00130 * 00131 * motor control functions 29.7.2005 stochri 00132 * void Go(int distance) 00133 * void Turn(int degree) 00134 * 00135 * unsigned char Wheelspeed[2] measured Wheelspeed by interupt 00136 * 00137 * Ver. Date Author Comments 00138 * ------- ---------- -------------- ------------------------------------------ 00139 * sto1 29.07.2005 stochri asuro library with motor control functions 00140 * ------- ---------- -------------- ------------------------------------------ 00141 *****************************************************************************/ 00142 /**************************************************************************** 00143 * 00144 * File Name: asuro.c 00145 * Project : asuro library "Robotrixer Buxtehude" 00146 * 00147 * Description: modifications made in following function: 00148 * 00149 * Go (int distance, int speed) Added Speed and Odometrie 00150 * Turn (int degree, int speed) Added Speed and Odometrie 00151 * 00152 * modifications in Sleep, SIG_OUTPUT_COMPARE2, PollSwitch, LineData 00153 * 00154 * Ver. Date Author Comments 00155 * ------- ---------- -------------- ------------------------------ 00156 * And1 31.07.2005 Andun See above 00157 * ------- ---------- -------------- ------------------------------ 00158 * 00159 *****************************************************************************/ 00160 /**************************************************************************** 00161 * 00162 * File Name: asuro.c 00163 * Project : asuro library 00164 * 00165 * Description: modifications made in following functions: 00166 * 00167 * void PrintInt(int wert) 00168 * 00169 * unsigned char Wheelspeed[2] removed 00170 * 00171 * Ver. Date Author Comments 00172 * ------- ---------- -------------- ------------------------------------------ 00173 * 2.60 28.09.2005 m.a.r.v.i.n doxygen comments 00174 * ------- ---------- -------------- ------------------------------------------ 00175 *****************************************************************************/ 00176 /**************************************************************************** 00177 * 00178 * File Name: asuro.c 00179 * Project : asuro library 00180 * 00181 * Description: modifications made in following functions: 00182 * 00183 * SIGNAL (SIG_ADC) 00184 * void PrintInt(int wert) 00185 * 00186 * 00187 * Ver. Date Author Comments 00188 * ------- ---------- -------------- ------------------------------------------ 00189 * 2.61 20.11.2006 m.a.r.v.i.n SIGNAL (SIG_ADC): static Variable toggle initialisiert 00190 * auf False (Bug report von Rolf_Ebert) 00191 * PrintInt: Initialisierung text String kann zu Fehler 00192 * beim Flashen mit RS232/IR Adapter fuehren 00193 * (Bug report von francesco) 00194 * ------- ---------- -------------- ------------------------------------------ 00195 *****************************************************************************/ 00196 /**************************************************************************** 00197 * 00198 * File Name: asuro.c 00199 * Project : asuro library 00200 * 00201 * Description: new functions has been added: 00202 * 00203 * void uart_putc(unsigned char zeichen) 00204 * void SerPrint(unsigned char *data) 00205 * void SetMotorPower(int8_t left_speed, int8_t right_speed ) 00206 * void sound(uint16_t freq, uint16_t duration_msec, uint8_t amplitude) 00207 * 00208 * Description: modifications made in following functions: 00209 * 00210 * void Go(int distance, int power) 00211 * void Turn(int degree, int speed) 00212 * void PrintInt(int wert) 00213 * 00214 * 00215 * Ver. Date Author Comments 00216 * ------- ---------- -------------- ------------------------------------------ 00217 * 2.70 07.01.2007 stochri new functions: 00218 * uart_putc: send single character 00219 * SerPrint: send 0-terminated string 00220 * SetMotorPower: set Motor speed and direction 00221 * sound: Sound Ausgabe ueber Motor PWM 00222 * Go: distance in mm 00223 * Turn: comments 00224 * m.a.r.v.i.n PrintInt: SerWrite ersetzt durch SerPrint 00225 * ------- ---------- -------------- ------------------------------------------ 00226 *****************************************************************************/ 00227 00228 00229 #ifndef ASURO_H 00230 #define ASURO_H 00231 00232 #include <avr/io.h> 00233 #include <avr/interrupt.h> 00234 #ifndef SIGNAL 00235 #include <avr/signal.h> // header file obsolete in actual avr-libc 00236 #include <inttypes.h> 00237 #endif 00238 #include <stdlib.h> 00239 00240 #define FALSE 0 00241 #define TRUE 1 00242 00243 #define OFF 0 00244 #define ON 1 00245 00246 #define GREEN 1 00247 #define RED 2 00248 #define YELLOW 3 00249 00250 /* neue Funktionen und Variablen*/ 00251 #define LEFT 0 00252 #define RIGHT 1 00253 #define CENTER 2 00254 00255 /* Definitionen fuer Taster 00256 * (Zaehlweise der Taster erfolgt von libks nachs rechts, 00257 * wenn der Asuro mit den Tastern nach oben zeigt) */ 00258 #define K1 (1<<5) 00259 #define K2 (1<<4) 00260 #define K3 (1<<3) 00261 #define K4 (1<<2) 00262 #define K5 (1<<1) 00263 #define K6 (1<<0) 00264 00265 /* --- Globale Variablen -----------------------------------*/ 00269 extern const char version[5]; 00270 00271 /* 00272 * Tastsensor Wert bei Interrupt Betrieb. 0=keine Taste, 1= Taste gedrueckt 00273 */ 00282 extern volatile int switched; 00283 00284 /* 00285 * Odometriesensor Zaehler bei Interrupt Betrieb. 00286 * encoder[0] links, encoder[1] = rechts. 00287 */ 00295 extern volatile int encoder[2]; 00296 00297 /* 00298 * Counter fuer 36kHz. 00299 * Wird in der Interrupt Funktion SIG_OVERFLOW2 hochgezaehlt\n 00300 * und in der Sleep() Funktion abgefragt. 00301 */ 00302 extern volatile unsigned char count36kHz; 00303 00304 /* 00305 * Sytemzeit in ms. 00306 * Wird in der Interrupt Funktion SIG_OVERFLOW2 hochgezaehlt 00307 * und in der Gettime() Funktion verwendet. 00308 */ 00309 extern volatile unsigned long timebase; 00310 00311 /* 00312 * Odometrie Sensor Abfrage im Interrupt Betrieb. 00313 * Wird in der Interrupt Funktion SIG_ADC abgefragt, 00314 * in der EncoderInit() und EncoderStart() Funktion gesetzt 00315 * und in der EncoderStop() Funktion geloescht. 00316 */ 00317 extern volatile int autoencode; 00318 00319 /* --- Funktions Prototypen -----------------------------------*/ 00320 00326 void Init(void); 00327 00328 /**************** Zeit- und Delay Funktionen time.c *********/ 00333 unsigned long Gettime(void); 00339 void Msleep(int ms); 00345 void Sleep(unsigned char time36kHz); 00346 00347 /**************** Low Level Encoder Funktionen encoder_low.c */ 00353 void EncoderInit(void); 00360 void EncoderSet(int setl,int setr); 00365 void EncoderStop(void); 00370 void EncoderStart(void); 00371 00372 /**************** Encoder Funktionen encoder.c **************/ 00383 void GoTurn(int distance, int degree, int speed); 00384 // aus Nostalgiegruenden Defines fuer alte Funktionsnamen 00385 #define Go(distance,speed) GoTurn(distance,0,speed) 00386 #define Turn(degree,speed) GoTurn(0,degree,speed) 00387 00388 /**************** A/D Wandler Funktionen adc.c **************/ 00394 int Battery(void); 00400 void LineData(unsigned int *data); 00406 void OdometryData(unsigned int *data); 00407 00408 // aus Nostalgiegruenden Defines fuer alte Funktionsnamen 00409 #define Batterie Battery 00410 #define OdometrieData OdometryData 00411 00412 /**************** LED Funktionen led.c **********************/ 00418 inline void StatusLED(unsigned char color); 00424 inline void FrontLED(unsigned char status); 00431 void BackLED(unsigned char left, unsigned char right); 00432 00433 /**************** Low Level Motorsteuerungs Funktionen motor_low.c */ 00441 inline void MotorDir(unsigned char left_dir, unsigned char right_dir); 00449 inline void MotorSpeed(unsigned char left_speed, unsigned char right_speed); 00450 00451 /**************** Motorsteuerungs Funktionen motor.c **************/ 00452 void SetMotorPower(int8_t leftpwm, int8_t rightpwm); 00453 00454 /******************** Low Level UART Funktionen uart.c ************/ 00461 void SerWrite(unsigned char *data,unsigned char length); 00469 void SerRead(unsigned char *data, unsigned char length, unsigned int timeout); 00470 00471 /**************** Print Funktionen serielle Ausgabe print.c ********/ 00472 void UartPutc(unsigned char zeichen); 00473 void SerPrint(char *data); 00474 void PrintInt(int wert); 00475 void PrintLong(long wert); 00476 /**************** Print Funktionen Floting Point printf.c ********/ 00477 void PrintFloat (float wert, char vorkomma, char nachkomma); 00478 00479 /**************** Taster Funktionen switches.c ******************/ 00485 unsigned char PollSwitch (void); 00492 void StartSwitch(void); 00499 void StopSwitch(void); 00500 00501 /**************** Soundausgabe sound.c **************************/ 00502 void Sound(uint16_t freq, uint16_t duration_msec, uint8_t amplitude); 00503 00504 /* ----------- END ------------ */ 00505 00506 00507 /* --------------- INTERNAL ------------- */ 00508 #define GREEN_LED_ON PORTB |= GREEN_LED 00509 #define GREEN_LED_OFF PORTB &= ~GREEN_LED 00510 #define RED_LED_ON PORTD |= RED_LED 00511 #define RED_LED_OFF PORTD &= ~RED_LED 00513 #define FWD (1 << PB5) 00514 #define RWD (1 << PB4) 00515 #define BREAK 0x00 00516 #define FREE (1 << PB4) | (1 << PB5) 00518 #define IRTX (1 << PB3) 00519 #define GREEN_LED (1 << PB0) 00520 #define RED_LED (1 << PD2) 00522 #define PWM (1 << PB1) | (1 << PB2) 00523 #define RIGHT_DIR (1 << PB4) | (1 << PB5) 00524 #define LEFT_DIR (1 << PD4) | (1 << PD5) 00526 #define SWITCHES (1 << PD3) 00527 #define SWITCH_ON PORTD |= SWITCHES 00528 #define SWITCH_OFF PORTD &= ~SWITCHES 00530 #define BATTERIE (1 << MUX0) | (1 << MUX2) 00531 #define SWITCH (1 << MUX2) 00532 #define IR_LEFT (1 << MUX0) | (1 << MUX1) 00533 #define IR_RIGHT (1 << MUX1) 00534 #define FRONT_LED (1 << PD6) 00536 #define ODOMETRIE_LED (1 << PD7) 00537 #define ODOMETRIE_LED_ON PORTD |= ODOMETRIE_LED 00538 #define ODOMETRIE_LED_OFF PORTD &= ~ODOMETRIE_LED 00540 #define WHEEL_LEFT (1 << MUX0) 00541 #define WHEEL_RIGHT 0 00543 #endif /* ASURO_H */