Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
executable file 1613 lines (1399 sloc) 36.9 KB
#include "main.h"
#ifndef F_CPU
#define F_CPU 16000000
#endif
//#include "/usr/local/avr/include/avr/delay.h"
uint8_t leftWheelCounter = 0;
uint8_t rightWheelCounter = 0;
//uint16_t leftRevolutionsCounter = 0;
//uint16_t rightRevolutionsCounter = 0;
uint16_t leftDistanceCounter = 0;
uint16_t rightDistanceCounter = 0;
//uint8_t camPanLSwitch = 0;
//uint8_t camPanRSwitch = 0;
//uint8_t camTiltLSwitch = 0;
//uint8_t camTiltRSwitch = 0;
int main(void)
{
// usart stuff
RXcompleted = 0; // Flag, String komplett empfangen
TXcompleted = 1; // Flag, String komplett gesendet
setStarter(42); //42 = '*'
setTerminator(35); //35 = '#'
redLEDtoggle = 0; // toggle for showing receiving traffic on a LED
leftWheelCounter = 0;
rightWheelCounter = 0;
// leftRevolutionsCounter = 0;
// rightRevolutionsCounter = 0;
leftDistanceCounter = 0;
rightDistanceCounter = 0;
//-----------------
// I/O definitions
//-----------------
// switch port C bits to input / output
//
// Bit0 = yellow LED
// Bit1 = flashlight
// Bit2 = ultrasonic trigger
// Bit3 = ultrasonic echo (input)
//
DDRC = (1 << DDC0) | (1 << DDC1) | (1 << DDC2) | (1 << DDC4) | (1 << DDC5) | (1 << DDC6) | (1 << DDC7);
// DDRB is the SPI / I2C Port and set at init_spi in micromag.c
// PB6 is used for checking if the micromag modul is connected. High = connected.
// switch some bits on port A to input (camera pan tilt end switches)
//DDRA &= ~((1 << DDA0) | (1 << DDA1) | (1 << DDA2) | (1 << DDA3));
// switch port L (all PINS) to output [drive motor 1 to 3]
DDRL = 0xff;
// switch some bits on port D to output [read LED, motor 4]
DDRD |= (1 << DDD5) | (1 << DDD6) | (1 << DDD7);
// switch some bits on port G to output [motor 4 pwm]
DDRG |= (1<<PIN5);
// switch port H (all PINS) to output [servos / RGB LEDs]
DDRH = 0xff;
// switch some bits on port E to output [2 more servos]
DDRE |= (1 << DDE3) | (1 << DDE4);
// red LED on. Now we know, that the program runs.
redLED(ON);
// yelow LED off
yellowLED(OFF);
// green LED off
greenLED(OFF);
// flashlight off
relais(OFF);
// turn all drive motor bits off (except PWM bits)
// motor 1
PORTL &= ~(1<<PIN0);
PORTL &= ~(1<<PIN1);
// motor 2
PORTL &= ~(1<<PIN2);
PORTL &= ~(1<<PIN3);
// motor 3
PORTL &= ~(1<<PIN6);
PORTL &= ~(1<<PIN7);
// motor 4
PORTD &= ~(1<<PIN6);
PORTD &= ~(1<<PIN7);
//-------------------------------------------------------------
// no interrupts please!
// this is *here* for setting the interrupt control registers
//-------------------------------------------------------------
cli();
// init AD converter with Interrrupt etc.
// initADC();
// turn OFF "power saving mode" for AD converter (turn on power for ADC)
PRR0 &= ~(1<<PRADC);
// switch some bits on port J to input
//
// Bit3 = Motor 1 Encoder 1
// Bit4 = Motor 1 Encoder 2
// Bit5 = Motor 2 Encoder 1
// Bit6 = Motor 2 Encoder 2
DDRJ &= ~((1 << DDJ3) | (1 << DDJ4) | (1 << DDJ5) | (1 << DDJ6));
// switch some bits on port K to input
//
// Bit0 = Cam R Tilt Endswitch
// Bit1 = Cam L Tilt Endswitch
// Bit2 = Cam R Pan Endswitch
// Bit3 = Cam L Pan Endswitch
//DDRK &= ~((1 << DDK0) | (1 << DDK1) | (1 << DDK2) | (1 << DDK3));
/*
//----------------------------------------------------------------------------
// Set the "Pin Change Interrupt Control Register"
// -> any change on any enabled PCINT15:8 pin will now cause an interrupt!
//----------------------------------------------------------------------------
PCICR = (1 << PCIE1);
// activate the pins which can cause an interrupt
// At this time use only the FORWARD sensor to generate an interrupt
// PCMSK1 = (1 << PCINT12) | (1 << PCINT13) | (1 << PCINT14) | (1 << PCINT15);
PCMSK1 = (1 << PCINT12) | (1 << PCINT15);
//----------------------------------------------------------------------------
// Set the "Pin Change Interrupt Control Register"
// -> any change on any enabled PCINT23:16 pin will now cause an interrupt!
//----------------------------------------------------------------------------
PCICR = (1 << PCIE2);
// activate the pins which can cause an interrupt
// At this time use only the FORWARD sensor to generate an interrupt
PCMSK2 = (1 << PCINT16) | (1 << PCINT17) | (1 << PCINT18) | (1 << PCINT19);
*/
//----------------------------------------------------------------------------
// initialzie SPI stuff
//----------------------------------------------------------------------------
// turn OFF "power saving mode" for SPI (serial peripheral interface)!
// (turn on power for SPI)
PRR0 &= ~(1<<PRSPI);
init_spi();
// initialize the PWM timer (with compare value 100) [this is the motor speed!]
// This value is changed by the mrs programm, when value is read from ini-file!
// 100 * 64 µs = 6400 µs = 6,4 ms
//
// drive motor 1
setPWMwidth(1, 60);
// drive motor 2
setPWMwidth(2, 60);
// drive motor 3
setPWMwidth(3, 60);
// drive motor 4
setPWMwidth(4, 60);
// start the motor PWM timers
startPWM();
// initialize the servo PWM timers
// This value will be changed by the direcs programm, when value is read from ini-file!
setServoPosition(1, 0);
setServoPosition(2, 0);
setServoPosition(3, 0);
setServoPosition(4, 0);
setServoPosition(5, 0);
setServoPosition(6, 0);
// start the servo PWM timer
startPWMServo(1);
startPWMServo(2);
startPWMServo(3);
startPWMServo(4);
startPWMServo(5);
startPWMServo(6);
unsigned char servoPosition = 0;
unsigned char servoPWM123isOFF = 0;
unsigned char servoPWM4isOFF = 0;
unsigned char servoPWM56isOFF = 0;
//-----------------------------------------------------
//-----------------------------------------------------
// UART 3 konfigurieren
UBRR3H = (unsigned char) (USART_BAUD_SELECT >> 8);
UBRR3L = (unsigned char) USART_BAUD_SELECT;
// enable UART3 and Interrupts for RX and TX
UCSR3B |= (1<<RXCIE3) | (1<<RXEN3) | (1<<TXEN3);
//----------------------------------------------------------------------------
// enable global interrupts
//----------------------------------------------------------------------------
sei();
// Endlose Hauptschleife
while(1)
{
// Wurde ein kompletter String empfangen und ist der Buffer ist leer?
if (RXcompleted == 1)
{
// Everything's fine, so reset the watchdog timer (wdt).
// wdt_reset();
// ja, dann String lesen und uart_rx_flag löschen
get_string(stringbuffer);
//--------------------------
// check what was received
//--------------------------
// RESET / INIT
if (strcmp(stringbuffer, "*re#") == 0)
{
// turn all drive motor bits off (except PWM bits)
PORTL &= ~(1<<PIN0);
PORTL &= ~(1<<PIN1);
PORTL &= ~(1<<PIN2);
PORTL &= ~(1<<PIN3);
PORTL &= ~(1<<PIN6);
PORTL &= ~(1<<PIN7);
PORTD &= ~(1<<PIN6);
PORTD &= ~(1<<PIN7);
// flashlight off
relais(OFF);
// red LED off. Know we know, that the program on the PC/Mac has initialised the Atmel
redLED(OFF);
// setServoPosition(1, 17); // <- exact position now in the mrs.ini!
// setServoPosition(2, 19); // <- exact position now in the mrs.ini!
// setServoPosition(3, 23); // <- exact position now in the mrs.ini!
// setServoPosition(4, 19); // <- exact position now in the mrs.ini!
// setServoPosition(5, 19); // <- exact position now in the mrs.ini!
// setServoPosition(6, 22); // <- exact position now in the mrs.ini!
// answer with "ok"
// this answer is used to see if the robot is "on"
put_string("*re#");
// e n a b l e watchdog!
watchdog(ENABLE);
}
else
// SLEEP (and turn off watchdog)
if (strcmp(stringbuffer, "*sl#") == 0)
{
// turn all drive motor bits off (except PWM bits)
PORTL &= ~(1<<PIN0);
PORTL &= ~(1<<PIN1);
PORTL &= ~(1<<PIN2);
PORTL &= ~(1<<PIN3);
PORTL &= ~(1<<PIN6);
PORTL &= ~(1<<PIN7);
PORTD &= ~(1<<PIN6);
PORTD &= ~(1<<PIN7);
// flashlight off
relais(OFF);
// red LED off
redLED(OFF);
// d i s a b l e watchdog!
watchdog(DISABLE);
// answer
put_string("*sl#");
}
else
// READ_SENSOR_1
if (strcmp(stringbuffer, "*s1#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR1) );
}
else
// READ_SENSOR_2
if (strcmp(stringbuffer, "*s2#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR2) );
}
else
// READ_SENSOR_3
if (strcmp(stringbuffer, "*s3#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR3) );
}
else
// READ_SENSOR_4
if (strcmp(stringbuffer, "*s4#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR4) );
}
else
// READ_SENSOR_5
if (strcmp(stringbuffer, "*s5#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR5) );
}
else
// READ_SENSOR_6
if (strcmp(stringbuffer, "*s6#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR6) );
}
else
// READ_SENSOR_7 (24 V supply)
if (strcmp(stringbuffer, "*s7#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR7) );
}
else
// READ_SENSOR_8 (12 V supply)
if (strcmp(stringbuffer, "*s8#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSOR8) );
}
else
// READ_SENSOR_16
// ultra sonic sensor !!
if (strcmp(stringbuffer, "*s16#") == 0)
{
// read ADC and send answer over serial port
sendUInt( ultraschall_messung() );
}
else
// READ 3D COMPASS CONNECTION
if (strcmp(stringbuffer, "*cc#") == 0)
{
// check if micromag is connected to Atmel-Board (PB6 = high)
if ( bit_is_set(PINB,PIN6) )
{
put_string("*cc#");
}
else
{
// not connected.
put_string("*er#");
}
}
else
// READ_AXIS_X
if (strcmp(stringbuffer, "*cx#") == 0)
{
// check if micromag is connected to Atmel-Board (PB6 = high)
if ( bit_is_set(PINB,PIN6) )
{
// read axis from the micromag3 sensor and send answer over serial port
sendUInt( readMicromag(READ_AXIS_X) );
}
else
{
// not connected.
sendUInt(0);
}
}
else
// READ_AXIS_Y
if (strcmp(stringbuffer, "*cy#") == 0)
{
// check if micromag is connected to Atmel-Board (PB6 = high)
if ( bit_is_set(PINB,PIN6) )
{
// read axis from the micromag3 sensor and send answer over serial port
sendUInt( readMicromag(READ_AXIS_Y) );
}
else
{
// not connected.
sendUInt(0);
}
}
else
// READ_AXIS_Z
if (strcmp(stringbuffer, "*cz#") == 0)
{
// check if micromag is connected to Atmel-Board (PB6 = high)
if ( bit_is_set(PINB,PIN6) )
{
// read axis from the micromag3 sensor and send answer over serial port
sendUInt( readMicromag(READ_AXIS_Z) );
}
else
{
// not connected.
sendUInt(0);
}
}
else
// READ_MOTOR_SENSOR1
if (strcmp(stringbuffer, "*ms1#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSORMOTOR1) );
}
else
// READ_MOTOR_SENSOR2
if (strcmp(stringbuffer, "*ms2#") == 0)
{
// read ADC and send answer over serial port
sendUInt( readADC(SENSORMOTOR2) );
}
else
// READ_MOTOR_DISTANCE1
if (strcmp(stringbuffer, "*dd1#") == 0)
{
// send driven distance of motor 1 encoder over serial port
sendUInt( rightDistanceCounter );
}
else
// READ_MOTOR_DISTANCE2
if (strcmp(stringbuffer, "*dd2#") == 0)
{
// send driven distance of motor 2 encoder over serial port
sendUInt( leftDistanceCounter );
}
else
// RESET_MOTOR_DISTANCE1
if (strcmp(stringbuffer, "*id1#") == 0)
{
// init driven distance of motor 1 (encoder)
rightDistanceCounter = 0;
rightWheelCounter = 0;
// answer with "ok"
put_string("*ok#");
}
else
// RESET_MOTOR_DISTANCE2
if (strcmp(stringbuffer, "*id2#") == 0)
{
// init driven distance of motor 2 (encoder)
leftDistanceCounter = 0;
leftWheelCounter = 0;
// answer with "ok"
put_string("*ok#");
}
else
// FLASHLIGHT ON
if (strcmp(stringbuffer, "*f0on#") == 0)
{
relais(ON);
yellowLED(ON);
put_string("*f0on#");
}
else
// FLASHLIGHT OFF
if (strcmp(stringbuffer, "*f0of#") == 0)
{
relais(OFF);
yellowLED(OFF);
put_string("*f0of#");
}
else
// MOTOR1_OFF
if (strcmp(stringbuffer, "*mp1of#") == 0)
{
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// answer with "ok"
put_string("*mp1of#");
}
else
// MOTOR 1 CLOCKWISE = forward
if (strcmp(stringbuffer, "*md1cw#") == 0)
{
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// set Motor1 B bit
PORTL |= (1<<PIN1);
// answer with "ok"
put_string("*md1cw#");
}
else
// MOTOR 1 COUNTERCLOCKWISE = backward
if (strcmp(stringbuffer, "*md1cc#") == 0)
{
// set Motor1 A bit
PORTL |= (1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// answer with "ok"
put_string("*md1cc#");
}
else
// MOTOR1_SPEED_SET
if (strncmp(stringbuffer, "*mv1", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string and set speed
setPWMwidth(1, atoi(stringbuffer));
// answer with "ok"
put_string("*mv1#");
}
else
// MOTOR2_OFF
if (strcmp(stringbuffer, "*mp2of#") == 0)
{
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// answer with "ok"
put_string("*mp2of#");
}
else
// MOTOR 2 CLOCKWISE = forward
if (strcmp(stringbuffer, "*md2cw#") == 0)
{
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// set Motor2 B bit
PORTL |= (1<<PIN3);
// answer with "ok"
put_string("*md2cw#");
}
else
// MOTOR 2 COUNTERCLOCKWISE = backward
if (strcmp(stringbuffer, "*md2cc#") == 0)
{
// set Motor2 A bit
PORTL |= (1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// answer with "ok"
put_string("*md2cc#");
}
else
// MOTOR2_SPEED_SET
if (strncmp(stringbuffer, "*mv2", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string and set speed
setPWMwidth(2, atoi(stringbuffer));
// answer with "ok"
put_string("*mv2#");
}
else
// MOTOR3_OFF
if (strcmp(stringbuffer, "*mp3of#") == 0)
{
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// answer with "ok"
put_string("*mp3of#");
}
else
// MOTOR 3 CLOCKWISE = forward
if (strcmp(stringbuffer, "*md3cw#") == 0)
{
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// set Motor3 B bit
PORTL |= (1<<PIN7);
// answer with "ok"
put_string("*md3cw#");
}
else
// MOTOR 3 COUNTERCLOCKWISE = backward
if (strcmp(stringbuffer, "*md3cc#") == 0)
{
// set Motor3 A bit
PORTL |= (1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// answer with "ok"
put_string("*md3cc#");
}
else
// MOTOR3_SPEED_SET
if (strncmp(stringbuffer, "*mv3", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string and set speed
setPWMwidth(3, atoi(stringbuffer));
// answer with "ok"
put_string("*mv3#");
}
else
// MOTOR4_OFF
if (strcmp(stringbuffer, "*mp4of#") == 0)
{
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*mp4of#");
}
else
// MOTOR 4 CLOCKWISE = forward
if (strcmp(stringbuffer, "*md4cw#") == 0)
{
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// set Motor4 B bit
PORTD |= (1<<PIN7);
// answer with "ok"
put_string("*md4cw#");
}
else
// MOTOR 4 COUNTERCLOCKWISE = backward
if (strcmp(stringbuffer, "*md4cc#") == 0)
{
// set Motor4 A bit
PORTD |= (1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*md4cc#");
}
else
// MOTOR4_SPEED_SET
if (strncmp(stringbuffer, "*mv4", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string and set speed
setPWMwidth(4, atoi(stringbuffer));
// answer with "ok"
put_string("*mv4#");
}
else
// SPEED_SET_ALLMOTORS
if (strncmp(stringbuffer, "*mv0", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string and set speed
setPWMwidth(1, atoi(stringbuffer));
setPWMwidth(2, atoi(stringbuffer));
setPWMwidth(3, atoi(stringbuffer));
setPWMwidth(4, atoi(stringbuffer));
// answer with "ok"
put_string("*mv0#");
}
else
// BOTSTOP
if (strcmp(stringbuffer, "*bst#") == 0)
{
// MOTOR 1 OFF
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// MOTOR 2 OFF
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// MOTOR 3 OFF
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// MOTOR 4 OFF
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*bst#");
}
else
// BOTWAIT
if (strcmp(stringbuffer, "*bwa#") == 0)
{
// MOTOR 1 OFF
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// MOTOR 2 OFF
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// MOTOR 3 OFF
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// MOTOR 4 OFF
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*bwa#");
}
else
// BOTSTART = "bot go"
if (strcmp(stringbuffer, "*bgo#") == 0)
{
// MOTOR 1 CLOCKWISE = forward
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// set Motor1 B bit
PORTL |= (1<<PIN1);
// MOTOR 2 CLOCKWISE = forward
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// set Motor2 B bit
PORTL |= (1<<PIN3);
// MOTOR 3 CLOCKWISE = forward
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// set Motor3 B bit
PORTL |= (1<<PIN7);
// MOTOR 4 CLOCKWISE = forward
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// set Motor4 B bit
PORTD |= (1<<PIN7);
// answer with "ok"
put_string("*bgo#");
}
else
// BOTFORWARD = "bot drive forward"
if (strcmp(stringbuffer, "*bdf#") == 0)
{
// MOTOR 1 CLOCKWISE = forward
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// set Motor1 B bit
PORTL |= (1<<PIN1);
// MOTOR 2 CLOCKWISE = forward
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// set Motor2 B bit
PORTL |= (1<<PIN3);
// MOTOR 3 CLOCKWISE = forward
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// set Motor3 B bit
PORTL |= (1<<PIN7);
// MOTOR 4 CLOCKWISE = forward
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// set Motor4 B bit
PORTD |= (1<<PIN7);
// answer with "ok"
put_string("*bdf#");
}
else
// BOTBACKWARD = "bot drive backward"
if (strcmp(stringbuffer, "*bdb#") == 0)
{
// MOTOR 1 COUNTERCLOCKWISE = backward
// set Motor1 A bit
PORTL |= (1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// MOTOR 2 COUNTERCLOCKWISE = backward
// set Motor2 A bit
PORTL |= (1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// MOTOR 3 COUNTERCLOCKWISE = backward
// set Motor3 A bit
PORTL |= (1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// MOTOR 4 COUNTERCLOCKWISE = backward
// set Motor4 A bit
PORTD |= (1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*bdb#");
}
else
// BOTLEFT = "bot drive left"
if (strcmp(stringbuffer, "*bdl#") == 0)
{
// MOTOR 1 COUNTERCLOCKWISE = backward
// set Motor1 A bit
PORTL |= (1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// MOTOR 2 CLOCKWISE = forward
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// set Motor2 B bit
PORTL |= (1<<PIN3);
// MOTOR 3 CLOCKWISE = forward
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// set Motor3 B bit
PORTL |= (1<<PIN7);
// MOTOR 4 COUNTERCLOCKWISE = backward
// set Motor4 A bit
PORTD |= (1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*bdl#");
}
else
// BOTRIGHT = "bot drive right"
if (strcmp(stringbuffer, "*bdr#") == 0)
{
// MOTOR 1 CLOCKWISE = forward
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// set Motor1 B bit
PORTL |= (1<<PIN1);
// MOTOR 2 COUNTERCLOCKWISE = backward
// set Motor2 A bit
PORTL |= (1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// MOTOR 3 COUNTERCLOCKWISE = backward
// set Motor3 A bit
PORTL |= (1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// MOTOR 4 CLOCKWISE = forward
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// set Motor4 B bit
PORTD |= (1<<PIN7);
// answer with "ok"
put_string("*bdr#");
}
else
// TURNLEFT = "bot turn left"
if (strcmp(stringbuffer, "*btl#") == 0)
{
// MOTOR 1 CLOCKWISE = forward
// delete Motor1 A bit
PORTL &= ~(1<<PIN0);
// set Motor1 B bit
PORTL |= (1<<PIN1);
// MOTOR 2 COUNTERCLOCKWISE = backward
// set Motor2 A bit
PORTL |= (1<<PIN2);
// delete Motor2 B bit
PORTL &= ~(1<<PIN3);
// MOTOR 3 CLOCKWISE = forward
// delete Motor3 A bit
PORTL &= ~(1<<PIN6);
// set Motor3 B bit
PORTL |= (1<<PIN7);
// MOTOR 4 COUNTERCLOCKWISE = backward
// set Motor4 A bit
PORTD |= (1<<PIN6);
// delete Motor4 B bit
PORTD &= ~(1<<PIN7);
// answer with "ok"
put_string("*btl#");
}
else
// TURNRIGHT = "bot turn right"
if (strcmp(stringbuffer, "*btr#") == 0)
{
// MOTOR 1 COUNTERCLOCKWISE = backward
// set Motor1 A bit
PORTL |= (1<<PIN0);
// delete Motor1 B bit
PORTL &= ~(1<<PIN1);
// MOTOR 2 CLOCKWISE = forward
// delete Motor2 A bit
PORTL &= ~(1<<PIN2);
// set Motor2 B bit
PORTL |= (1<<PIN3);
// MOTOR 3 COUNTERCLOCKWISE = backward
// set Motor3 A bit
PORTL |= (1<<PIN6);
// delete Motor3 B bit
PORTL &= ~(1<<PIN7);
// MOTOR 4 CLOCKWISE = forward
// delete Motor4 A bit
PORTD &= ~(1<<PIN6);
// set Motor4 B bit
PORTD |= (1<<PIN7);
// answer with "ok"
put_string("*btr#");
}
else
// SET SERVO 1
if (strncmp(stringbuffer, "*sv1", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string
servoPosition = atoi(stringbuffer);
if (servoPosition == 0)
{
// stop PWM for servos 1, 2, 3 (due to same Atmel ports)
servoPWM123isOFF = 1;
stopPWMServo(1);
}
else
{
// turn on PWM if it was turned off before
if (servoPWM123isOFF == 1)
{
servoPWM123isOFF = 0;
// start PWM for servos 1, 2, 3 (due to same Atmel ports)
startPWMServo(1);
}
// set PWM
setServoPosition(1, servoPosition);
}
// answer with "ok"
put_string("*sv1#");
}
else
// SET SERVO 2
if (strncmp(stringbuffer, "*sv2", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string
servoPosition = atoi(stringbuffer);
if (servoPosition == 0)
{
// stop PWM for servos 1, 2, 3 (due to same Atmel ports)
servoPWM123isOFF = 1;
stopPWMServo(2);
}
else
{
// turn on PWM if it was turned off before
if (servoPWM123isOFF == 1)
{
servoPWM123isOFF = 0;
// start PWM for servos 1, 2, 3 (due to same Atmel ports)
startPWMServo(2);
}
// set PWM
setServoPosition(2, servoPosition);
}
// answer with "ok"
put_string("*sv2#");
}
else
// SET SERVO 3
if (strncmp(stringbuffer, "*sv3", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string
servoPosition = atoi(stringbuffer);
if (servoPosition == 0)
{
// stop PWM for servos 1, 2, 3 (due to same Atmel ports)
servoPWM123isOFF = 1;
stopPWMServo(3);
}
else
{
// turn on PWM if it was turned off before
if (servoPWM123isOFF == 1)
{
servoPWM123isOFF = 0;
// start PWM for servos 1, 2, 3 (due to same Atmel ports)
startPWMServo(3);
}
// set PWM
setServoPosition(3, servoPosition);
}
// answer with "ok"
put_string("*sv3#");
}
else
// SET SERVO 4
if (strncmp(stringbuffer, "*sv4", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string
servoPosition = atoi(stringbuffer);
if (servoPosition == 0)
{
// stop PWM for servos 4
servoPWM4isOFF = 1;
stopPWMServo(4);
}
else
{
// turn on PWM if it was turned off before
if (servoPWM4isOFF == 1)
{
servoPWM4isOFF = 0;
// start PWM for servo 4
startPWMServo(4);
}
// set PWM
setServoPosition(4, servoPosition);
}
// answer with "ok"
put_string("*sv4#");
}
else
// SET SERVO 5
if (strncmp(stringbuffer, "*sv5", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string
servoPosition = atoi(stringbuffer);
if (servoPosition == 0)
{
// stop PWM for servos 5, 6 (due to same Atmel ports)
servoPWM56isOFF = 1;
stopPWMServo(5);
}
else
{
// turn on PWM if it was turned off before
if (servoPWM56isOFF == 1)
{
servoPWM56isOFF = 0;
// start PWM for servos 5, 6 (due to same Atmel ports)
startPWMServo(5);
}
// set PWM
setServoPosition(5, servoPosition);
}
// answer with "ok"
put_string("*sv5#");
}
else
// SET SERVO 6
if (strncmp(stringbuffer, "*sv6", 4) == 0)
{
// change first chars for upcoming string conversion
stringbuffer[0] = '0';
stringbuffer[1] = '0';
stringbuffer[2] = '0';
stringbuffer[3] = '0';
// get value from string
servoPosition = atoi(stringbuffer);
if (servoPosition == 0)
{
// stop PWM for servos 5, 6 (due to same Atmel ports)
servoPWM56isOFF = 1;
stopPWMServo(6);
}
else
{
// turn on PWM if it was turned off before
if (servoPWM56isOFF == 1)
{
servoPWM56isOFF = 0;
// start PWM for servos 5, 6 (due to same Atmel ports)
startPWMServo(6);
}
// set PWM
setServoPosition(6, servoPosition);
}
// answer with "ok"
put_string("*sv6#");
}
} // RXcompleted
} // while (1)
// this line is never reached!
return 0;
}
/*
switch (value)
{
case MOTOR1_OFF:
case MOTOR1_CLOCKWISE:
case MOTOR1_COUNTERCLOCKWISE:
case MOTOR1_SPEED_SET:
case MOTOR2_OFF:
case MOTOR2_CLOCKWISE:
case MOTOR2_COUNTERCLOCKWISE:
case MOTOR2_SPEED_SET:
case MOTOR3_OFF:
case MOTOR3_CLOCKWISE: // cam pan R
case MOTOR3_COUNTERCLOCKWISE: // cam pan L
case MOTOR3_SPEED_SET:
case MOTOR4_OFF:
case MOTOR4_CLOCKWISE: // cam tilt top
case MOTOR4_COUNTERCLOCKWISE: // cam tilt bottom
case MOTOR4_SPEED_SET:
case SPEED_SET_ALLMOTORS:
case BOTSTOP:
case BOTWAIT:
case BOTSTART:
case BOTFORWARD:
case BOTBACKWARD:
case BOTLEFT:
case BOTRIGHT:
case BOTSTOP:
case BOTWAIT:
case BOTTURNLEFT:
case BOTTURNRIGHT:
case FLASHLIGHT_ON:
case FLASHLIGHT_OFF:
case SET_SERVO1:
// wait for the (second) value to set the pwm!
value = UsartReceive();
setServoPosition(1, value);
break;
case SET_SERVO2:
// wait for the (second) value to set the pwm!
value = UsartReceive();
setServoPosition(2, value);
break;
case SET_SERVO3:
// wait for the (second) value to set the pwm!
value = UsartReceive();
setServoPosition(3, value);
break;
case SET_SERVO4:
// wait for the (second) value to set the pwm!
value = UsartReceive();
setServoPosition(4, value);
break;
case SET_SERVO5:
// wait for the (second) value to set the pwm!
value = UsartReceive();
setServoPosition(5, value);
break;
case SET_SERVO6:
// wait for the (second) value to set the pwm!
value = UsartReceive();
setServoPosition(6, value);
break;
case READ_CONTACT1:
// contact cam tilt R/BOTTOM
// send 1 Byte (8 bit!)
UsartTransmit( (uint8_t) bit_is_set(PINK,PIN3) );
break;
case READ_CONTACT2:
// contact cam tilt L/TOP
// send 1 Byte (8 bit!)
UsartTransmit( (uint8_t) bit_is_set(PINK,PIN2) );
break;
case READ_CONTACT3:
// contact cam pan R
// send 1 Byte (8 bit!)
UsartTransmit( (uint8_t) bit_is_set(PINK,PIN1) );
break;
case READ_CONTACT4:
// contact cam pan L
// send 1 Byte (8 bit!)
UsartTransmit( (uint8_t) bit_is_set(PINK,PIN0) );
break;
}
*/
void sendUInt(uint16_t value)
{
uint8_t length = 0;
// start the answer string to send with a '*'
stringbuffer[0] = starter;
// convert int to ascii (to Basis 10)
// (but don't overwrite the first char which is the 'starter' *.)
itoa(value, stringbuffer+1, 10);
// get the length of the string
length = strlen(stringbuffer);
// add m string terminator '#' at the end of the buffer
stringbuffer[length] = terminator;
// String mit \0 terminieren
stringbuffer[length+1] = 0;
// send answer
put_string(stringbuffer);
}
void redLED(uint8_t state)
{
if (state == ON)
{
// red LED on
// (low active!)
PORTD &= ~(1<<PIN5);
}
else
{
// red LED off
// (low active!)
PORTD |= (1<<PIN5);
}
}
void yellowLED(uint8_t state)
{
if (state == ON)
{
// yellow LED on
// (low active!)
PORTC &= ~(1<<PIN0);
}
else
{
// yellow LED off
// (low active!)
PORTC |= (1<<PIN0);
}
}
void greenLED(uint8_t state)
{
if (state == ON)
{
// green LED on
// (low active!)
PORTC &= ~(1<<PIN4);
}
else
{
// green LED off
// (low active!)
PORTC |= (1<<PIN4);
}
}
void relais(uint8_t state)
{
if (state == ON)
{
// relais on
PORTC |= (1<<PIN1);
}
else
{
// relais off
PORTC &= ~(1<<PIN1);
}
}
/*
SIGNAL(PCINT1_vect) // todo: replace this old SIGNAL by ISR with correct _vect name!!
{
//
// The circumference of one wheel is 428 mm (42,8 cm).
//
// The disk has 120.which generate 2 interrupts per slit (one low-to-high transistion and one high-to-low transistion).
// So we have 240 interrupts per circumference
//
// At each interrupt the wheel moves: 195 mm / 240 = 1,783 mm.
// For 10 mm (1 cm) we need: 10 mm / 1,783mm = 5,60747 interrupts -> After 6 interruupts the robot moves 10 mm (1 cm).
//static uint8_t value = 0;
//----------------------------
// if left wheel moves
//----------------------------
if ( bit_is_set(PINJ,PIN3) )
{
leftWheelCounter++;
if (leftWheelCounter == 6)
{
leftDistanceCounter++;
leftWheelCounter = 0;
//
// TEST TEST TEST
//
// if (value == 0)
// {
// value = 1;
// PORTC &= ~(1<<PIN0); // yellow led ON (low)
// }
// else
// {
// value = 0;
// PORTC |= (1<<PIN0); // yelow LED off (low active -> high)
// }
//
// TEST TEST TEST
//
}
}
//----------------------------
// if right wheel moves
//----------------------------
if ( bit_is_set(PINJ,PIN6) )
{
rightWheelCounter++;
if (rightWheelCounter == 6)
{
rightDistanceCounter++;
rightWheelCounter = 0;
}
}
}
*/
/*
SIGNAL(PCINT2_vect) // todo: replace this old SIGNAL by ISR with correct _vect name!!
{
//----------------------------
// if Cam Pan L switch set
//----------------------------
if ( bit_is_set(PINK,PIN0) )
{
// turn off MOTOR3 pan L bit (A)
// PORTL &= ~(1<<PIN6);
// camPanLSwitch = 1;
}
else
{
// camPanLSwitch = 0;
}
//----------------------------
// if Cam Pan R switch set
//----------------------------
if ( bit_is_set(PINK,PIN1) )
{
// turn off MOTOR3 pan R bit (B)
// PORTL &= ~(1<<PIN7);
// camPanRSwitch = 1;
}
else
{
// camPanRSwitch = 0;
}
//----------------------------
// if Cam Tilt L/TOP switch set
//----------------------------
if ( bit_is_set(PINK,PIN2) )
{
// turn off MOTOR4 tilt L bit (A)
// PORTD &= ~(1<<PIN6);
// camTiltLSwitch = 1;
}
else
{
// camTiltLSwitch = 0;
}
//----------------------------
// if Cam Tilt R/BOTTOM switch set
//----------------------------
if ( bit_is_set(PINK,PIN3) )
{
// turn off MOTOR4 tilt R bit (B)
// PORTD &= ~(1<<PIN7);
// camTiltRSwitch = 1;
}
else
{
// camTiltRSwitch = 0;
}
}
*/
void long_delay(uint16_t ms)
{
for (; ms>0; ms--) _delay_ms(1);
}
void watchdog(uint8_t state)
{
/*
// Disable global interrupts.
// This is here for setting the interrupt control registers
cli();
// Reset the watchdog timer (wdt).
// When the wdt is enabled, a call to this instruction is required before the timer expires,
// otherwise a watchdog-initiated device reset will occur.
wdt_reset();
//--------------------
// Enable watchdog
//--------------------
if (state == ENABLE)
{
// start timed sequence
WDTCSR |= (1<<WDCE) | (1<<WDE);
// set new prescaler (time-out) to 64K (65536 cycles) which means a time-out after 0.5 s
// WDTCSR = (1<<WDP2) | (1<<WDP1); // 12K = 1.0s
WDTCSR = (1<<WDP2) | (1<<WDP0); // 64K = 0.5s
// And here we enable the watchdog interrupt!
// So if a time-out occurs, the correspondig interrupt routine is executed
WDTCSR |= (1<<WDIE);
// Enable global interrupts
sei();
return;
}
//--------------------
// Disable watchdog
//--------------------
// Clear WDRF in MCUSR
MCUSR &= ~(1<<WDRF);
// write logical 1 to WDCE and WDE
// keep old prescaler setting to prevent unintentional time-out
WDTCSR |= (1<<WDCE) | (1<<WDE);
// turn off WDT
WDTCSR = 0x00;
// Enable global interrupts
sei();
*/
}
/*
SIGNAL(WDT_vect)
{
// turn all drive motor bits off (except PWM bits)
PORTL &= ~(1<<PIN0);
PORTL &= ~(1<<PIN1);
PORTL &= ~(1<<PIN2);
PORTL &= ~(1<<PIN3);
PORTL &= ~(1<<PIN6);
PORTL &= ~(1<<PIN7);
PORTD &= ~(1<<PIN6);
PORTD &= ~(1<<PIN7);
// turn flashlight on !
relais(ON);
}
*/
You can’t perform that action at this time.