int main (void)
{
uint16_t heading;
int16_t pitch, roll;
uint16_t headingIntegerPart, headingFractionPart;
int16_t pitchIntegerPart, pitchFractionPart;
int16_t rollIntegerPart, rollFractionPart;
// initialize the hardware stuff we use
InitTimer ();
InitUART ();
i2c_init ();
fdevopen (UART0_PutCharStdio, UART0_GetCharStdio);
// set the LED port for output
LED_DDR |= LED_MASK;
printf ("\nHoneywell HMC6343 I2C Compass Test\n\n");
// Flash the LED for 1/2 a second...
turnOnLED ();
ms_spin (500);
turnOffLED ();
while (1) // outer loop is once every 250 ms (more or less)
{
// send the HEADING command to the compass
i2c_start_wait (COMPASS_ADDRESS_WRITE);
i2c_write (COMPASS_HEADING_COMMAND);
// now read the response
i2c_rep_start (COMPASS_ADDRESS_READ);
heading = (i2c_readAck () * 256) + i2c_readAck ();
pitch = (i2c_readAck () * 256) + i2c_readAck ();
roll = (i2c_readAck () * 256) + i2c_readNak ();
i2c_stop ();
headingIntegerPart = heading / 10;
headingFractionPart = heading - (headingIntegerPart * 10);
pitchIntegerPart = pitch / 10;
pitchFractionPart = pitch - (pitchIntegerPart * 10);
if (pitchFractionPart < 0)
pitchFractionPart *= -1;
rollIntegerPart = roll / 10;
rollFractionPart = roll - (rollIntegerPart * 10);
if (rollFractionPart < 0)
rollFractionPart *= -1;
printf ("Heading: %3d.%1d Pitch: %3d.%1d Roll: %3d.%1d\n", headingIntegerPart, headingFractionPart, pitchIntegerPart, pitchFractionPart, rollIntegerPart, rollFractionPart);
turnOnLED ();
ms_spin (100);
turnOffLED ();
ms_spin (150);
}
// we'll never get here...
return 0;
}
// Funktion zum senden der Menuepunkte ueber die serielle Schnittstelle
void menu_puts (void *arg, char *name) { // Menu/Sende Funktion
uart_put_string(arg, D_Stepper); // Uebergebenen String an Stepper senden
// Befehl auf Display ausgeben
lcd_clrscr();
lcd_puts("Sent: ");
lcd_puts(arg);
lcd_puts("\n");
ms_spin(100);
//if ((UCSR1A & (1 << RXC1)))
uart_rx(D_Stepper); // Antwort des Stepper empfangen
ms_spin(1000); // Antwort noch eine weile Anzeigen
}
void InitSensors( void )
{
TWCR = 0; // Force TWI off (AD4 & 5 share the TWI pins)
// For DDRx pins, 1 = output, 0 = input.
// For input pins, PORTx = 0 = pullup disabled
// PORTx = 1 = pullup enabled
DDRC = 0; // All input pins
PORTC = 0; // All pullups off
{
// Setup the input pins on Port D
uns8 pins = ( 1 << PgmSelect1 )
| ( 1 << PgmSelect2 )
| ( 1 << PgmSelect4 )
| ( 1 << BlackButton );
ClearBits( DDRD, pins ); // Configure inputs
ClearBits( PORTD, pins ); // Disable pullups
}
ADC_Init( ADC_PRESCALAR_AUTO );
// Delay for a short time to allow the ADC to initialize
ms_spin( 1 );
}
// Uebersetzung Schrittmotorkarte
void switch_Stepper (char * str_rx) {
const char* pOptions[] = { // Array mit bekannten Befehlen
"#", // 0 - Stepper Karte hat Befehl erkannt
"E", // 1 - Stepper Karte meldet Error
"!CLS", // 2 - Clear Screen (Debugging)
"Test", // 3 - Test (Debugging)
0
};
switch (FindStringInArray(str_rx, pOptions, 1)) { // String gegen bekannte Antworten pruefen
case 0: // 0 - Stepper Karte hat Befehl erkannt
lcd_puts("Erfolgreich\n");
break;
case 1: // 1 - Stepper Karte meldet Error
lcd_puts("Error\n");
uart_put_string("1\r\n", D_RapidForm);
break;
case 2: // 2 - Clear Screen (Debugging)
lcd_clrscr();
break;
case 3: // 3 - Test (Debugging)
lcd_puts("Test bestanden\n");
break;
default:
ms_spin(10);
}
}
int main( void )
{
int i;
int led = 0;
uint16_t prevSwitches = 0;
InitHardware();
InitADC();
InitMotors();
eeprom_read_block( &gMemParam, &gEEParam, sizeof( gMemParam ));
if ( gMemParam.thresh_hi == 0xFF )
{
gMemParam.thresh_hi = 0x80;
}
if ( gMemParam.thresh_lo == 0xFF )
{
gMemParam.thresh_lo = 0x10;
}
#if CFG_LOG_USE_STDIO
fdevopen( UART0_PutCharStdio, UART0_GetCharStdio );
LogInit( stdout );
#endif
// The first handle opened for read goes to stdin, and the first handle
// opened for write goes to stdout. So u0 is stdin, stdout, and stderr
Log( "*****\n" );
Log( "***** Line Maze program\n" );
Log( "*****\n" );
LCD_Init( 2, 16 );
LCD_Printf( " SRS Sample Bot " );
LCD_MoveTo( 0, 1 );
LCD_Printf( "Second line" );
Log( "\n" );
MENU_Init( gTopMenu );
LED_OFF( GREEN );
while( 1 )
{
uint16_t switches;
uint8_t pinc;
uint8_t pind;
int8_t error;
LED_TOGGLE( GREEN );
led++;
if ( led >= 6 )
{
led = 0;
}
error = GetLineError();
if ( MENU_IsActive() )
{
MENU_Event( MENU_EVENT_TIMER );
}
else
{
if ( MENU_IsModified() )
{
eeprom_write_block( &gMemParam, &gEEParam, sizeof( gMemParam ));
MENU_ClearModified();
LCD_Clear();
LCD_Printf( "EEPROM Updated\n" );
Log( "EEPROM Updated\n" );
ms_spin( 1000 );
}
//switches = EXP_TransferWord( ~( 1 << ( led + 2 )), EXP_OUT_LED_MASK );
switches = EXP_TransferWord( 0, 0 );
Log( "SW:%04x ", switches );
if ( switches != prevSwitches )
{
LCD_Clear();
prevSwitches = switches;
}
for ( i = 0; i < 8; i++ )
{
Log( "%02x ", gLineADC[ i ]);
}
//Log( "C: %02x\n", PINC );
switch (( switches & 0xF0 ) >> 4 )
{
case 0:
{
LCD_MoveTo( 0, 0 );
LCD_Printf( " SRS Sample Bot " );
LCD_MoveTo( 0, 1 );
LCD_Printf( "Second line" );
break;
}
case 1:
{
LCD_MoveTo( 0, 0 );
LCD_Printf( "Joy: %c%c%c%c%c",
(( switches & 0x4000 ) == 0 ) ? 'L' : ' ',
(( switches & 0x2000 ) == 0 ) ? 'R' : ' ',
(( switches & 0x0800 ) == 0 ) ? 'U' : ' ',
(( switches & 0x0400 ) == 0 ) ? 'D' : ' ',
(( switches & 0x1000 ) == 0 ) ? 'X' : ' ' );
LCD_MoveTo( 0, 1 );
LCD_Printf( "S1:%d S2:%d S3:%d",
( switches & 0x0200 ) == 0,
( switches & 0x0100 ) == 0,
( PIND & ( 1 << 6 )) == 0 );
break;
}
case 2:
{
LCD_MoveTo( 0, 0 );
LCD_Printf( "L %02x %02x %02x %02x %02x",
gLineADC[ 0 ],
gLineADC[ 1 ],
gLineADC[ 2 ],
gLineADC[ 3 ],
gLineADC[ 4 ] );
LCD_MoveTo( 0, 1 );
LCD_Printf( "E %02x %02x %3d B %02x", gADC[ 0 ], gADC[ 7 ], error, gADC[ 6 ] );
break;
}
default:
{
LCD_MoveTo( 0, 0 );
LCD_Printf( "Setting: %d", ( switches & 0xF0 ) >> 4 );
break;
}
}
pinc = PINC;
pind = PIND;
Log( " QL:%d%d QR:%d%d EC-L:%d EC-R:%d Err:%2d L:%5b H:%5b\n",
( ENCODER_L_A_PIN & ENCODER_L_A_MASK ) != 0,
( ENCODER_L_B_PIN & ENCODER_L_B_MASK ) != 0,
( ENCODER_R_A_PIN & ENCODER_R_A_MASK ) != 0,
( ENCODER_R_B_PIN & ENCODER_R_B_MASK ) != 0,
gEncoderCountL,
gEncoderCountR,
error, gLowMask, gHighMask );
}
// Tick rate is 100/sec so waiting for 50 waits for 1/2 sec
for ( i = 0; i < 50; i++ )
{
WaitForTimer0Rollover();
CheckSwitches();
#if 1
if ( UART0_IsCharAvailable() )
{
char ch = UART0_GetChar();
if ( ch == ' ' )
{
DebugKey();
}
else
{
Log( "Read: '%c'\n", ch );
}
}
#endif
}
}
return 0;
}
static inline void MENU_Delay( unsigned msec )
{
ms_spin( msec );
} // MENU_Delay
int main (void)
{
// speed_t wrist;
// speed_t finger;
uint16_t fingerPosition;
// initialize the hardware stuff we use
InitTimer ();
ADC_Init (ADC_PRESCALAR_AUTO);
InitMotors ();
InitTimerUART ();
// set the LED port for output
LED_DDR |= LED_MASK;
// Flash the LED for 1/2 a second...
ledOn ();
ms_spin (500);
ledOff ();
Log ("*****\n");
Log ("***** Bioloid Gripper Test\n");
Log ("***** Copyright 2007 HUVrobotics\n");
Log ("*****\n");
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
fingerPosition = ADC_Read (7);
Log ("Speed = 0... Position = %4d\n", fingerPosition);
SetMotorSpeed (SPEED_OFF, 0);
while (fingerPosition < MAX_FINGER)
{
fingerPosition = ADC_Read (7);
}
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
ms_spin (1000);
fingerPosition = ADC_Read (7);
Log ("Speed = 255... Position = %4d\n", fingerPosition);
SetMotorSpeed (SPEED_OFF, 255);
while (fingerPosition > MIN_FINGER)
{
fingerPosition = ADC_Read (7);
}
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
Log ("Done... Position = %4d\n", fingerPosition);
/* ms_spin (1000);
ledOn ();
for (wrist = SPEED_OFF; wrist < 255; wrist++) {
SetMotorSpeed (wrist, SPEED_OFF);
ms_spin (25);}
ms_spin (1000);
ledOff ();
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
ms_spin (1000);
ledOn ();
for (wrist = SPEED_OFF; wrist > 0; wrist--) {
SetMotorSpeed (wrist, SPEED_OFF);
ms_spin (25);}
ms_spin (1000);
ledOff ();
SetMotorSpeed (SPEED_OFF, SPEED_OFF);
*/
return 0;
}
int main(void)
{
InitTimer();
InitUART();
LED_DDR |= LED_MASK;
LCD_Init( CFG_LCD_NUM_LINES, CFG_LCD_NUM_COLUMNS );
LCD_PutStr( "LCD Test\n" );
LCD_PutStr( ">Line 2<\n" );
fdevopen( UART0_PutCharStdio, UART0_GetCharStdio );
printf( "*****\n" );
printf( "***** LCD Test program\n" );
printf( "*****\n" );
DDRB &= ~SW_ALL;
PORTB |= SW_ALL;
while( 1 )
{
LED_PORT |= LED_MASK;
ms_spin( 100 );
LED_PORT &= ~LED_MASK;
ms_spin( 100 );
LED_PORT |= LED_MASK;
ms_spin( 100 );
LED_PORT &= ~LED_MASK;
ms_spin( 700 );
LCD_MoveTo(0,1);
uint8_t pinb = PINB;
if ( pinb & SW_A )
{
printf("A is high ");
LCD_PutChar('a');
}
else
{
printf("A is low ");
LCD_PutChar('A');
}
if ( pinb & SW_B )
{
printf("B is high ");
LCD_PutChar('b');
}
else
{
printf("B is low ");
LCD_PutChar('B');
}
if ( pinb & SW_C )
{
printf("C is high\n");
LCD_PutChar('c');
}
else
{
printf("C is low\n");
LCD_PutChar('C');
}
}
return 0;
}
