int main (void) {
unsigned char kommando[32]; // Kommandozeilenbuffer
unsigned char* k_sp = kommando; // und dazugehöriger Pointer
volatile struct buffer put_buffer, get_buffer; // Sende- u. Empfangsbuffer
buffer_init(put_buffer);
buffer_init(get_buffer);
put_bp=&put_buffer;
get_bp=&get_buffer;
DDRD = 0xE0; // Port D, Pin 6,7,8 als Ausgang
tim0_init(); // Initialisiere Timer_0
tim1_init(); // Initialisiere Timer_1
tim2_init(); // Initialisiere Timer_2
uart_init(); // Initialisiere serielle Schnittstelle
adc_init(); // Initialisiere Analog-Digitalwandler ADC
uart_puts(version); // Startmeldung/Version ausgeben
beep(1000);
uart_puts("\n> "); // newline und prompt
UCSRB |= (1<<RXCIE); // Empfaenger-Interrupts freigeben
sei(); // Interrupts global freigeben
while(1) {
while(buffer_size(get_bp)){
*k_sp = buffer_read(get_bp);
uart_putc(*k_sp);
if(*k_sp == '\n' || *k_sp == '\r'){
*k_sp = 0x00;
k_sp=kommando;
uart_putc('\n');
// uart_puts(k_sp); // Ausgabe für Testwecke
parser(k_sp);
}
else
k_sp++;
// hier noch Bufferoverflow abfangen
}
if(status_LED2 == 's'){
PORTD ^= 1<<PD6; // toggle LED2
my_delay_ms(delay); // warte delay ms
PORTD ^= 1<<PD6; // toggle LED2
status_LED2 = '0'; // Status_Flag auf Aus
}
if(status_LED2 == 't'){
my_delay_ms(delay); // warte delay ms
PORTD ^= 1<<PD6; // toggle LED2
}
}
/* wird nie erreicht */
return 0;
}
int hardware_init(void)
{
RCC_Configuration_motor();
//NVIC_Configuration_motor();
adc_config();
vout_init();
usb_init();
tim2_init();
rt_kprintf("ADC System initialized!\r\n");
return 0;
}
/* ==================================================================================================================================
init function
====================================================================================================================================
*/
void init(void){
/* Inicjalizacjia zegarów */
SystemClock_Config();
/* Inicjalizacja portu LED */
led_port_init();
/* Inicjalizacja uartu2 */
usart2_init();
//Powitanie
usart2_WriteS("-->> NUCLEO L46 <<--\n\rTEST\n\rCompile time: ");
usart2_WriteS(__TIME__);
/* USART 1 */
// usart1_init();
/*USART 3 */
// usart3_init();
/*UART 4 */
// usart_stat = uart4_init();
/*UART 5 */
// usart_stat = uart5_init();
/* TIM2 */
//procent++;
tim2_stat = tim2_init();
/* TIM3 */
tim3_stat = tim3_init();
/* TIM5 INIT */
//tim5_stat = tim5_init();
/* I2C1 INIT */
//i2c1_status = i2c_init(I2C1, 0x00702991 , I2C1_PORT_SDA , I2C1_PIN_SDA , I2C1_PORT_SCL, I2C1_PIN_SCL , I2C1_GPIO_AF);
/*GPIO INIT FOR TIME MESURE */
//gpio_init(GPIOB , GPIO_PIN_8 , GPIO_MODE_OUTPUT_PP , GPIO_PULLUP);
/* INIT SPI */
spi2_nRF_init();
spi3_nRF_init();
}
int main ( void )
{
//TRISA = 0b0010000000000000;
TRISA = 0b00001111;
TRISAbits.TRISA12 = 0;
TRISCbits.TRISC1 = 0;
TRISCbits.TRISC2 = 0;
TRISCbits.TRISC3 = 0;
TRISCbits.TRISC4 = 0;
//TRISD = 0;
TRISD = 0b0000000000010000; // RD11: DIR2; RD8: PWM2
//TRISE = 0b00001000;
TRISE = 0b00001000; // RE6: DIR1; RE0: PWM1
TRISEbits.TRISE7 = 0;
//LCD an
_PCFG16 = 1; // AN16 is digital
_PCFG17 = 1; // AN17 is digital
_PCFG18 = 1; // AN18 is digital
_PCFG19 = 1; // AN19 is digital
_PCFG20 =1;
_PCFG31=1;
//pwm analog select
_PCFG24=1;
_PCFG30=1;
//RE4 for lcd r/s
_PCFG28=1;
TRISEbits.TRISE4 = 0;
AD1PCFGHbits.PCFG28 = 1;
AD1PCFGHbits.PCFG27 = 1;
AD1PCFGHbits.PCFG30 = 1;
AD1PCFGHbits.PCFG24 = 1;
AD1PCFGH = 0x0020;
TRISAbits.TRISA13 = 1;
//set pwm low
PWM1=0;
PWM2=0;
SYS_Initialize ( ) ;
CLKDIVbits.FRCDIV = 0;
CLKDIVbits.PLLPOST = 0; // N2 = 2
CLKDIVbits.PLLPRE = 0; // N1 = 2
PLLFBD = (Fosc*N1_default*N2_default/Ffrc) - M_default; // M = 8 -> Fosc = 7.3728 MHz * 8 / 2 / 2 = 16 MHz
while(!OSCCONbits.LOCK); // Wait for PLL to lock
RCONbits.SWDTEN = 0; // Disable Watch Dog Timer
//TRISF = 0;
gpsLock = 0;
lcd_init();
print_lcd("Initializing");
//delay_ms(500);
//lcd_clear();
/*lcd test
char line1[] = " On Route ";
char line2[] = " Arrived ";
//send_command_byte(0xFF);
//while(1){send_command_byte(0xFF);send_data_byte(0);}
// delay_ms(2);
//send_command_byte(0x02); // Go to start of line 1
//send_command_byte(0b00001111); // Display: display on, cursor on, blink on
//send_command_byte(0b00000110); // Set entry mode: ID=1, S=0
//send_command_byte(0b00000001); // Clear display
print_lcd(line1);
delay_ms(5000);
lcd_clear();
print_lcd(line2);
//send_command_byte(0xC0); // Go to start of line 1
//while(1){send_command_byte(0b00000001);}
while(1);*/
/*int a;
long long int ct;
int i;
int j=0;*/
//i2c init
//uart init
UART_Initialize();
delayMs(100);
/* while(1){//test for delay ms configuration
//PORTDbits.RD1 = 1;
//delayUs(10);
//for(i = 0;i <7;i++);
delayMs(10);
PORTDbits.RD12 = 1;
//for (i = 0; i < 1000; i++);
//PORTDbits.RD1 = 0;
//for(i = 0;i <7;i++);
delayMs(10);
PORTDbits.RD12 = 0;
//for (i = 0; i < 1000; i++);
}*/
//ultrasonic test
/* while(1){
long double x;
delayMs(500);
PORTEbits.RE4 = 1; //TRIGGER HIGH
PORTDbits.RD5 = 1;
delay_us(10); //10uS Delay
lcd_clear();
ultraSonicEn = 1;
ultraSonicCount = 0;
PORTEbits.RE4 = 0; //TRIGGER LOW
PORTDbits.RD5 = 0;
while (!PORTDbits.RD4); //Waiting for Echo
IEC0bits.T2IE = 1; //enable timer
while(PORTDbits.RD4);//
IEC0bits.T2IE = 0; //disable timer
x = ultraSonicCount/TICKS_PER_METER;
sprintf(outputBuf,"%lf",x);
print_lcd(outputBuf);
}*/
//TX_str(endGPS);
//delayMs(3000);
//TX_str(startGPS);
/*TX_str(startGPShot);
delayMs(2000);
while ( !gpsLock ) {
TX_str(getGPS);
delayMs(500);
}*/
//TCP code
TX_str(openNet);
delayMs(5000);
TX_str(openConnection);
delayMs(5000);
// while(!BUTTON_IsPressed ( BUTTON_S3 ));
//TX_str(sprintf("%s%d\r",sendTCP, strlen("10")));
sprintf(outputBuf2,"2\n%lf,%lf\n\r",roverLog,roverLat );
sprintf(cmdBuf,"%s%d\r", sendTCP,strlen(outputBuf2));
TX_str(cmdBuf);
delayMs(3000);
TX_str(outputBuf2);
while(!waypointsReady || !gpsLock);
//while(1);
delayMs(7000);
lcd_clear();
print_lcd("waypoints locked");
i2c_init();
//i2c_write3(0x3c, 0x00, 0x70);
i2c_write3(0x3c, 0x00, 0b00011000);
//i2c_write3(0x3c, 0x01, 0b11000000);
i2c_write3(0x3c, 0x01, 0xA0);
i2c_write3(0x3c, 0x02, 0x00);
//timer init, do this after other initializations
motorDuty=0;
tim1_init();
tim2_init();
/*
double angleTolerance = 8.0;
motorDuty=2;
while (1){ //adjust
double angleDiff = headingDiff(0,roverHeading );
if (angleDiff > 0 ||abs(angleDiff) > 175 ){ //turn left
PWM1_DIR = 0; //left NOTE: 0 is forward, 1 is reverse
PWM2_DIR = 1; //right
} else{ // turn right
PWM1_DIR = 1;
PWM2_DIR = 0;
}
if (abs(angleDiff) < angleTolerance){
motorDuty = 0;
//break;
}else{
//motorDuty =4;
motorDuty =2;
}
delayMs(13);
updateHeading();
}*/
//hardcoded gps for tcpip test
/*while(1){
// PORTDbits.RD5 = 1;
// for (a = 0; a < (100/33); a++) {}
// PORTDbits.RD5 = 0;
// for (a = 0; a < (100/33); a++) {}
ct = 0;
//TMR2 = 0;//Timer Starts
delayMs(60);
PORTEbits.RE4 = 1; //TRIGGER HIGH
PORTDbits.RD5 = 1;
delay_us(15);
//10uS Delay
PORTEbits.RE4 = 0; //TRIGGER LOW
PORTDbits.RD5 = 0;
while (!PORTDbits.RD4){ //Waiting for Echo
ct++;
}
//T2CONbits.TON = 1;
while(PORTDbits.RD4) {
ct++;
}// {
//T2CONbits.TON = 0;
sprintf(outputBuf,"%lld", ct);
//a = TMR2;
//a = a / 58.82;
//long int p;
//for(p = 0; p <100000; p++);
}*/
//tim1_init();
//while ( 1 );
//UART_Initialize();
/* Infinite Loop */
/*
long int i;
while ( 1 )
{//test for delay ms configuration
//PORTDbits.RD1 = 1;
delayMs(10);
PORTDbits.RD12 = 1;
//for (i = 0; i < 1000; i++);
//PORTDbits.RD1 = 0;
delayMs(10);
PORTDbits.RD12 = 0;
//for (i = 0; i < 1000; i++);
}*/
//IEC0bits.T1IE = 0;
//IEC0bits.T1IE = 1;
//motorDuty =2;
motorStopFlag = 0; //ready to go
char tempBuf1[50];
int wapointsVisited;
double desiredHeading = 0;
double dToWaypoint = 99999.9;
double angleTolerance = 6.0;
for (wapointsVisited =0; wapointsVisited<numGPSpoints;wapointsVisited++ )
{
dToWaypoint = 99999.9;
while (1)
{
int f;
roverLog = 0;
roverLat = 0;
for(f = 0; f < ROVER_LEN; f++){
roverLog+=roverlog[f]/ROVER_LEN_D;
roverLat+=roverlat[f]/ROVER_LEN_D;
}
dToWaypoint = dist(convertGPSToDeg(roverLat),convertGPSToDeg(roverLog),convertGPSToDeg(gpsLat[wapointsVisited]),convertGPSToDeg(gpsLonge[wapointsVisited]));
if(dToWaypoint <= 3.0){break;} //go to next waypoint
desiredHeading = 330.0;
desiredHeading = bearing(convertGPSToDeg(roverLat),convertGPSToDeg(roverLog),convertGPSToDeg(gpsLat[wapointsVisited]),convertGPSToDeg(gpsLonge[wapointsVisited]));
updateHeading();
if (!(abs(headingDiff(desiredHeading,roverHeading )) < angleTolerance))
{
motorDuty = 0; //stop
delayMs(300);
while (1) //ADJUSTMENT LOOP
{
double angleDiff = headingDiff(desiredHeading,roverHeading );
//double dist = ultraSonicPing();
/*sprintf(tempBuf1,"%f",dist);
lcd_clear();
print_lcd(tempBuf1);*/
if (abs(angleDiff) < angleTolerance){
motorDuty = 0;
/*sprintf(tempBuf1,"%f",roverHeading);
IEC0bits.T1IE = 0;
print_lcd(tempBuf1);
IEC0bits.T1IE = 1;*/
break;
}
if (angleDiff > 0 || abs(angleDiff) > 175 ){ //turn right
/*PORTDbits.RD4 = 1; //right 0 is forwards, 1 i backwards
PORTDbits.RD2 = 1;
PORTDbits.RD8 = 0; //left*/
PWM1_DIR = 0; //left NOTE: 0 is forward, 1 is reverse
PWM2_DIR = 1; //right
} else{ // turn left
/*PORTDbits.RD4 = 0; //right 0 is forwards, 1 i backwards
PORTDbits.RD2 = 0;
PORTDbits.RD8 = 1; //left*/
PWM1_DIR = 1;
PWM2_DIR = 0;
}
motorDuty =3;
delayMs(15);
updateHeading();
// sprintf(tempBuf1,"%f",roverHeading);
// IEC0bits.T1IE = 0;
// lcd_clear();
// print_lcd(tempBuf1);
// IEC0bits.T1IE = 1;
//delayMs(1000);
//for(p = 0; p <100000; p++);
}
}
PWM1_DIR = 0; //left NOTE: 0 is forward, 1 is reverse
PWM2_DIR = 0;
motorDuty = 5;
ultraSonicDelayEnable = 1;
//frequency is around 20kHz
while (ultraSonicDelayCount < 60000){ //for 4 seconds poll ultrasonic and check for obsticles
long double x;
//PORTEbits.RE4 = 1; //TRIGGER HIGH
PORTDbits.RD5 = 1;
delay_us(10); //10uS Delay
lcd_clear();
ultraSonicEn = 1;
ultraSonicCount = 0;
//PORTEbits.RE4 = 0; //TRIGGER LOW
PORTDbits.RD5 = 0;
while (!PORTDbits.RD4); //Waiting for Echo
IEC0bits.T2IE = 1; //enable timer
while(PORTDbits.RD4);//
IEC0bits.T2IE = 0; //disable timer
ultraSonicEn = 0;
x = ultraSonicCount/TICKS_PER_METER;
if (x <= 1.4){
motorDuty = 0;
}else{
motorDuty = 5;
}
delayMs(200);
}
ultraSonicDelayEnable = 0;
ultraSonicDelayCount = 0;
}
motorDuty = 0;
delayMs(1000);
//IEC0bits.T1IE = 0;
lcd_clear();
char buf3[40];
sprintf(buf3, "reached %d", wapointsVisited);
print_lcd(buf3);
delayMs(3000);
//IEC0bits.T1IE = 1;
}
//IEC0bits.T1IE = 0;
lcd_clear();
print_lcd("ARRIVED!!!");
//IEC0bits.T1IE = 1;
while (1);
}
int main(void){
uint8_t *string; // string from UART2 & pointer to last full GPS answer
uint8_t lastGPSans[UART_BUF_DATA_SIZE] = {0};
int i;
rcc_clock_setup_in_hse_8mhz_out_72mhz();
// init systick (1ms)
systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8); // Systyck: 72/8=9MHz
systick_set_reload(STK_RVR_DEFAULT_VAL); // 9000 pulses: 1kHz
systick_interrupt_enable();
systick_counter_enable();
GPIO_init();
/*
// if PC11 connected to usb 1.5kOhm pull-up through transistor
rcc_periph_clock_enable(RCC_GPIOC);
gpio_set(GPIOC, GPIO11);
gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_2_MHZ,
GPIO_CNF_OUTPUT_PUSHPULL, GPIO11);
*/
usb_disconnect(); // turn off USB while initializing all
usbkeybrd_setup();
UART_init(USART2); // init GPS UART
#ifdef ULTRASONIC
tim2_init(); // ultrasonic timer
#endif
//tim4_init(); // beeper timer
/*
for (i = 0; i < 0x80000; i++)
__asm__("nop");
*/
usb_connect(); // turn on USB
GPS_send_start_seq();
init_adc_sensor();
// time (in milliseconds from MCU start) for trigger, adc & power LED status; power LED blink interval
// blink time: (1000ms - powerLEDblink) - LED ON
// GPSstatus_tm - timer for blinking by GPS LED if there's no GPS after timer is good
// powerLEDblink - LED blinking time (depends on power level)
uint32_t usbkbrdtm = 0, trigrtm = 0, powerLEDtm = 0, GPSstatus_tm = 0, powerLEDblink = 1;
// istriggered == 1 after ANY trigger's event (set it to 1 at start to prevent false events)
// GPSLEDblink - GPS LED blinking
uint8_t istriggered = 1, GPSLEDblink = 0;
iwdg_set_period_ms(50); // set watchdog timeout to 50ms
iwdg_start();
while(1){
if(Timer == 500) // turn off PPS LED after 500ms
gpio_set(LEDS_Y_PORT, LEDS_Y2_PIN);
poll_usbkeybrd();
if(usbkbrdtm != msctr){ // process USB not frequently than once per 1ms
process_usbkbrd();
usbkbrdtm = msctr;
}
#ifdef ULTRASONIC
poll_ultrasonic();
#endif
poll_ADC();
if((string = check_UART2())){
memcpy(lastGPSans, string, UART_BUF_DATA_SIZE);
GPS_parse_answer(string);
}
/*
if(msctr - trigrtm > 3000){
trigrtm = msctr;
for(i = 0; i < 3; ++i){ // IR or Laser
P("ADC");
put_char_to_buf('0' + i);
P(" val: ");
print_int(ADC_value[i]);
newline();
}
}*/
if(istriggered){ // there was any trigger event
if(msctr - trigrtm > TRIGGER_DELAY || trigrtm > msctr){ // turn off LED & beeper
istriggered = 0;
gpio_set(LEDS_Y_PORT, LEDS_Y1_PIN);
gpio_set(BEEPER_PORT, BEEPER_PIN);
trigger_ms = DIDNT_TRIGGERED;
adc_ms[0] = DIDNT_TRIGGERED;
adc_ms[1] = DIDNT_TRIGGERED;
#ifdef ULTRASONIC
ultrasonic_ms = DIDNT_TRIGGERED;
#endif
}
}else{
if(trigger_ms != DIDNT_TRIGGERED){ // Control Button pressed
trigrtm = msctr;
istriggered = 1;
P("Button time: ");
print_time(&trigger_time, trigger_ms);
if(*lastGPSans){
P("GPS last message: ");
send_msg((char*)lastGPSans);
newline();
}
}
for(i = 0; i < 2; ++i){ // IR or Laser
uint32_t adcms = adc_ms[i];
if(adcms == DIDNT_TRIGGERED) continue;
int32_t timediff = Timer - adcms;
if(timediff < 0) timediff += 1000;
// pause for noice removal
if(timediff > ADC_NOICE_TIMEOUT && !istriggered){
trigrtm = msctr;
istriggered = 1;
if(i == 0) P("Infrared");
else P("Laser");
/* P(" trig val: ");
print_int(ADC_trig_val[i]);*/
put_char_to_buf(' ');
//P(" time: ");
print_time(&adc_time[i], adcms);
}
}
#ifdef ULTRASONIC
if(ultrasonic_ms != DIDNT_TRIGGERED && !istriggered){
trigrtm = msctr;
istriggered = 1;
P("Ultrasonic time: ");
print_time(&ultrasonic_time, ultrasonic_ms);
}
#endif
if(istriggered){ // turn on Y1 LED
gpio_clear(LEDS_Y_PORT, LEDS_Y1_PIN);
//beep(); // turn on beeper
gpio_clear(BEEPER_PORT, BEEPER_PIN);
}
}
// check 12V power level (once per 1ms)
if(powerLEDtm != msctr){
uint16_t _12V = ADC_value[2];
if(_12V < GOOD_POWER_LEVEL){ // insufficient power? - blink LED R2
// calculate blink time only if there's [was] too low level
if(_12V < POWER_ALRM_LEVEL || powerLEDblink){
powerLEDblink = GOOD_POWER_LEVEL - _12V;
// critical level: power LED is almost OFF
if(_12V < POWER_CRITICAL_LEVEL) powerLEDblink = 990;
//if(powerLEDblink > 990) powerLEDblink = 990; // shadow LED not more than 0.99s
}
}else{ // power restored - LED R2 shines
if(powerLEDblink){
gpio_clear(LEDS_R_PORT, LEDS_R2_PIN);
powerLEDblink = 0;
}
powerLEDtm = msctr;
}
if(powerLEDblink){
if(GPIO_ODR(LEDS_R_PORT) & LEDS_R2_PIN){ // LED is OFF
if(msctr - powerLEDtm > powerLEDblink || msctr < powerLEDtm){ // turn LED ON
powerLEDtm = msctr;
gpio_clear(LEDS_R_PORT, LEDS_R2_PIN);
}
}else{
if(msctr - powerLEDtm > (1000 - powerLEDblink) || msctr < powerLEDtm){ // turn LED OFF
powerLEDtm = msctr;
gpio_set(LEDS_R_PORT, LEDS_R2_PIN);
}
}
}
}
// check GPS status to turn on/off GPS LED
if(current_time.H < 24){ // timer OK
if((GPS_status != GPS_VALID) || need_sync){
GPSLEDblink = 1;
}else{
GPSLEDblink = 0;
if((GPIO_ODR(LEDS_G_PORT) & LEDS_G1_PIN) == 0)
gpio_clear(LEDS_G_PORT, LEDS_G1_PIN); // turn ON G1 LED
}
if(GPSLEDblink){
if(msctr - GPSstatus_tm > 500 || msctr < GPSstatus_tm){
GPSstatus_tm = msctr;
if(GPIO_ODR(LEDS_G_PORT) & LEDS_G1_PIN){ // LED is OFF
gpio_clear(LEDS_G_PORT, LEDS_G1_PIN);
}else{
gpio_set(LEDS_G_PORT, LEDS_G1_PIN);
}
}
}
}else{ // something bad with timer - turn OFF G1 LED
if(!(GPIO_ODR(LEDS_G_PORT) & LEDS_G1_PIN)){
gpio_set(LEDS_G_PORT, LEDS_G1_PIN);
}
}
iwdg_reset(); // reset watchdog
}
}
