void lcd_signed(unsigned char line, signed int value, char digits)
{
lcd_instruction(line);
lcd_print(value, 10, SIGNED_ZEROS, digits);
}
unsigned int pick_heading()
{
int heading = 0;
char numpress = 0;
lcd_clear();
lcd_print("Choose input mode:\n");
lcd_print("1. Enter Specific\n");
lcd_print("2. Select Predefined\n");
do {
get_kpd_input();
} while(keypad!='1' && keypad!='2');
lcd_clear();
switch(keypad)
{
case '1': //specific
do {
numpress = 0;
lcd_clear();
heading = 0;
lcd_print("Enter heading:\n");
while(keypad != '#' || numpress == 0)
{
get_kpd_input();
printf("\r%d\n",keypad);
if(keypad == '*') break; //cancel
if(numpress < 3 && keypad != '#' && keypad != '*')
{
numpress++;
heading = 10*heading + (int)(keypad-48);
lcd_print("%d",keypad-48);
}
}
} while(heading >= 360 || keypad == '*');
lcd_clear();
return 10*heading;
break;
case '2': //predefined
lcd_print("2=N, 4=W, 6=E, 8=S\n");
while(1)
{
get_kpd_input();
switch(keypad)
{
case '2':
return 0;
case '4':
return 2700;
case '6':
return 900;
case '8':
return 1800;
}
}
break;
}
return -1;
}
void sortCheck()
{
//int a1, a2;
int arm0 = 0, arm1 = 1;
if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
{
arm0 = 1;
arm1 = 0;
}
if ((sort[ct % 2] == color[ct] || sort[ct % 2] == color[adj]))
{
if (armCount>0)
{
ct = indicator[sort[ct % 2]];
if (arm[arm0] == -1)
pickSort(arm0, ct % 2);
else if (arm[arm1] == -1)
pickSort(arm1, ct % 2);
if (armCount != 1 && term[ct][0] != -1 && term[ct][1] != -1 && sort[ct % 2] == -1)
{
if (arm[arm0] == color[ct])
sortDrop(arm1, ct % 2);
else if (arm[arm1] == color[ct])
sortDrop(arm0, ct % 2);
}
}
else ct = adj;
}
else if (arm[arm0] != color[adj] && arm[arm1] != color[adj])
{
if (armCount == 0 || (armCount == 1 && sort[ct % 2] != -1) || ((armCount == 2 || armCount == 1) && (term[adj][0] == color[adj] || term[adj][0] == -1) && (term[adj][1] == color[adj] || term[adj][1] == -1)))
{
if (armCount>0)
{
if(sort[ct % 2] != -1)
{
if (arm[arm0] == -1)
pickSort(arm0, ct % 2);
else if (arm[arm1] == -1)
pickSort(arm1, ct % 2);
}
}
traverseToSort(ct % 2 + 4, (ct + 1) % 2 + 4);
newSort();
if (ct == 4 || ct == 5)
newSort();
return;
}
else ct = adj;
}
else
{
if (armCount == 0 && ((visited[adj] == 1 && (term[adj][0] != -1 && term[adj][1] != -1)) || visited[adj] == 0) && sort[ct % 2] == -1)
if (arm[arm0] == color[adj])
sortDrop(arm1, ct % 2);
else sortDrop(arm0, ct % 2);
else if (armCount == 1 && (term[adj][0] == -1 || term[adj][1] == -1) && sort[ct % 2] != -1)
if (arm[arm0] == -1)
pickSort(arm0, ct % 2);
else pickSort(arm1, ct % 2);
ct = adj;
}
lcd_print(2,15,ct,1);
buzzer();
_delay_ms(1000);
adj = adjCount(ct);
}
/////////////////
// main routine
/////////////////
void main (void) {
int8_t ret1, ret2;
int16_t accX1, accY1, accZ1;
int16_t accX2, accY2, accZ2;
char buf[100];
uint8_t count=0, len=0;
/////////////////
// init peripherals
/////////////////
// disable interrupts
DISABLE_INTERRUPTS;
// switch to 16MHz (default is 2MHz)
CLK.CKDIVR.byte = 0x00;
// set default option bytes to assert bootloader is running
flash_OPT_default();
// init timer TIM3 for sleep and timeout (required by I2C)
tim3_init();
// init timer TIM4 for 1ms clock with interrupts
tim4_init();
// init I2C bus
i2c_init();
// init and reset LCD display
lcd_init();
// init pins for UART1 Rx(=PA4) and Tx(=PA5)
gpio_init(&PORT_A, PIN_4, INPUT_PULLUP_NOEXINT);
gpio_init(&PORT_A, PIN_5, OUTPUT_PUSHPULL_FAST);
// init UART1 to high speed (connected to PC on muBoard)
uart1_init(230400L);
// init LEDs on muBoard for visual feedback
GPIO_SET(PORT_H,PIN_2|PIN_3, 1);
gpio_init(&PORT_H, PIN_2|PIN_3, OUTPUT_PUSHPULL_FAST);
// enable interrupts
ENABLE_INTERRUPTS;
// init I2C routine pointers
I2C_routine();
// initialize sensors
do {
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_init(&bno055);
ret1 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret1 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret1 && USE_I2C_ADDR1);
do {
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_init(&bno055);
ret2 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret2 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret2 && USE_I2C_ADDR2);
/////////////////
// main loop
/////////////////
while (1) {
// every 1ms do
if (g_flagClock) {
g_flagClock = 0;
// every 10ms do
if (g_clock > 10) {
g_clock = 0;
// just to be sure
accX1 = accY1 = accZ1 = ret1 = 0;
accX2 = accY2 = accZ2 = ret2 = 0;
// read data from sensor 1
#if USE_I2C_ADDR1
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_read_accel_x(&accX1);
ret1 |= bno055_read_accel_y(&accY1);
ret1 |= bno055_read_accel_z(&accZ1);
if (ret1 != 0) {
accX1 = accY1 = accZ1 = 0;
}
#endif // USE_I2C_ADDR1
// read data from sensor 2
#if USE_I2C_ADDR2
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_read_accel_x(&accX2);
ret2 |= bno055_read_accel_y(&accY2);
ret2 |= bno055_read_accel_z(&accZ2);
if (ret2 != 0) {
accX2 = accY2 = accZ2 = 0;
}
#endif // USE_I2C_ADDR2
// send data to PC via UART1. Use SW FIFO for background operation
len = 0;
buf[len++] = (uint8_t)(accX1 >> 8); // x1-acc (MSB first)
buf[len++] = (uint8_t) accX1;
buf[len++] = (uint8_t)(accY1 >> 8); // y1-acc (MSB first)
buf[len++] = (uint8_t) accY1;
buf[len++] = (uint8_t)(accZ1 >> 8); // z1-acc (MSB first)
buf[len++] = (uint8_t) accZ1;
buf[len++] = (uint8_t)(accX2 >> 8); // x2-acc (MSB first)
buf[len++] = (uint8_t) accX2;
buf[len++] = (uint8_t)(accY2 >> 8); // y2-acc (MSB first)
buf[len++] = (uint8_t) accY2;
buf[len++] = (uint8_t)(accZ2 >> 8); // z2-acc (MSB first)
buf[len++] = (uint8_t) accZ2;
uart1_send_buf(len, buf);
// indicate I2C status via red LED (on=ok)
GPIO_SET(PORT_H,PIN_3, ret1|ret2);
// show life beat via green LED
if (++count > 20) {
count = 0;
GPIO_TOGGLE(PORT_H,PIN_2);
// print to LCD
sprintf(buf, "%02d %03d %03d %03d", (int) ret1, (int) accX1, (int) accY1, (int) accZ1);
lcd_print(1, 1, buf);
sprintf(buf, "%02d %03d %03d %03d", (int) ret2, (int) accX2, (int) accY2, (int) accZ2);
lcd_print(2, 1, buf);
}
} // loop 10ms
} // loop 1ms
} // main loop
//Function To Print Sesor Values At Desired Row And Coloumn Location on LCD
void print_sensor(char row, char coloumn,unsigned char channel)
{
ADC_Value = ADC_Conversion(channel);
lcd_print(row, coloumn, ADC_Value, 3);
}
void lcd_put()
{
lcd_print(lcd.str);
}
//-----------------------------------------------------------------------------
// Main Function
//-----------------------------------------------------------------------------
void main(void)
{
desired_range = 150;
// initialize board
Sys_Init();
putchar(' '); //the quotes in this line may not format correctly
PCA_Init();
ADC_Init();
Interrupt_Init();
Port_Init();
XBR0_Init();
SMB_Init();
putchar('\r');
//print beginning message
printf("En taro Adun, Executor!\r\n");
// set the PCA output to a neutral setting
steering_neutral();
ranger_neutral();
wait(); /* Let stuff warm up */
// Get the PD constants and target heading from the keypad
compass_kp = read_gain()/10.;
desired_heading = read_heading();
compass_kd = read_gain_high();
ranger_kp = read_gain_thrust()/10.;
ranger_kd = read_gain_thrust_high();
//Write the settings to the record
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
PCA0CPL1 = 0xFFFF - 3200;
PCA0CPH1 = ( 0xFFFF - 3200 ) >> 8 ;
// Start at time 0.
time = 0;
while(1) /* Infinite loop is go */
{
if ( nCounts == 50 )
{
// Update the LCD loop
lcd_clear();
lcd_print( "Batt level: %d\n", get_battery_voltage() );
lcd_print("Heading :%d\n", heading/10);
lcd_print("Alt: %d", range );
if (SS)
lcd_print("PAUSED\n");
else
lcd_print("\n");
lcd_print("# forHead, * forGain");
}
if ( read_keypad() != -1 )
{
// Check the keypad
pause();
if ( getKey() == '#' )
{
// If # was pressed, get a new heading
steering_neutral();
ranger_neutral();
desired_heading = read_heading();
time = 0;
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
}
else if( getKey() == '*' )
{
// If * was pressed, get new gain contants
steering_neutral();
ranger_neutral();
compass_kp = read_gain()/10.;
compass_kd = read_gain_high();
ranger_kp = read_gain_thrust()/10.;
ranger_kd = read_gain_thrust_high();
time = 0;
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
}
}
else
pause();
if(SS) //while the SS is off
{
//printf("The slide switch is OFF \r\n");
steering_neutral();
ranger_neutral();
}
if(!SS) //SS is On
{
//printf("The Slide switch is On \r\n");
if ( new_heading )
printf("%lu,%d,%d\r\n",
time,
compass_old_error,
ranger_old_error );
if (new_heading) //wait for 40ms
{
heading=read_compass();
//printf("The heading result is %d\r\n", heading);
new_heading=0;
Steering_Servo( );
}
if (new_range)
{
ReadRanger( );
Speed_Cont( ); //adjust motor speed
new_range=0; //reset ranger flag
}
}
}
}
int main(void)
{
//Konfiguration der Ausgänge bzw. Eingänge
//definition erfolgt in der config.h
DDRA = OUTA;
#if USE_SER_LCD
DDRC = OUTC;
#else
DDRC = OUTC;
#if PORTD_SCHALT
DDRD = OUTD;
#endif
#endif
// RoBue:
// Pullups einschalten
PORTA = (1 << PORTA0) | (1 << PORTA1) | (1 << PORTA2) | (1 << PORTA3) | (1 << PORTA4) | (1 << PORTA5) | (1 << PORTA6);
unsigned long a;
#if USE_SERVO
servo_init ();
#endif //USE_SERVO
usart_init(BAUDRATE); // setup the UART
#if USE_ADC
ADC_Init();
#endif
usart_write("\n\rSystem Ready\n\r");
usart_write("Compiliert am "__DATE__" um "__TIME__"\r\n");
usart_write("Compiliert mit GCC Version "__VERSION__"\r\n");
for(a=0;a<1000000;a++){asm("nop");};
//Applikationen starten
stack_init();
httpd_init();
telnetd_init();
//Spielerrei mit einem LCD
#if USE_SER_LCD
udp_lcd_init();
lcd_init();
// RoBue:
// LCD-Ausgaben:
lcd_clear();
lcd_print(0,0,"*AVR-NET-IO "Version"*");
lcd_print(2,0,"Counter: ");
lcd_print(3,0,"Zeit:");
#endif
//Ethernetcard Interrupt enable
ETH_INT_ENABLE;
#if USE_SER_LCD
// RoBue:
// IP auf LCD
lcd_print(1,0,"%1i.%1i.%1i.%1i",myip[0],myip[1],myip[2],myip[3]);
#endif
//Globale Interrupts einschalten
sei();
#if USE_CAM
#if USE_SER_LCD
lcd_print(1,0,"CAMERA INIT");
#endif //USE_SER_LCD
for(a=0;a<2000000;a++){asm("nop");};
cam_init();
max_bytes = cam_picture_store(CAM_RESELUTION);
#if USE_SER_LCD
back_light = 0;
lcd_print(1,0,"CAMERA READY");
#endif //USE_SER_LCD
#endif // -> USE_CAM
#if USE_NTP
ntp_init();
ntp_request();
#endif //USE_NTP
#if USE_WOL
wol_init();
#endif //USE_WOL
#if USE_MAIL
mail_client_init();
#endif //USE_MAIL
// Startwerte für ow_array setzen
#if USE_OW
uint8_t i = 0;
for (i=0;i<MAXSENSORS;i++){
ow_array[i]=OW_START;
}
for (i=MAXSENSORS;i<MAXSENSORS*3;i++){
ow_array[i]=OW_MINMAX;
}
DS18X20_start_meas( DS18X20_POWER_PARASITE, NULL );
for(a=0;a<1000000;a++){asm("nop");};
auslesen = 0;
minmax = 1;
#endif
//Hauptschlfeife
// *************
while(1)
{
#if USE_ADC
ANALOG_ON;
#endif
eth_get_data();
//Terminalcommandos auswerten
if (usart_status.usart_ready){
usart_write("\r\n");
if(extract_cmd(&usart_rx_buffer[0]))
{
usart_write("Ready\r\n\r\n");
}
else
{
usart_write("ERROR\r\n\r\n");
}
usart_status.usart_ready =0;
}
// RoBue:
// Counter ausgeben
#if USE_SER_LCD
lcd_print(2,9,"%4i",var_array[MAX_VAR_ARRAY-1]);
#endif
// RoBue:
// Uhrzeit bestimmen und auf LCD ausgeben
hh = (time/3600)%24;
mm = (time/60)%60;
ss = time%60;
#if USE_SER_LCD
lcd_print(3,7,"%2i:%2i:%2i",hh,mm,ss);
#endif
#if USE_HIH4000
var_array[VA_OUT_HIH4000] = ((var_array[VA_IN_HIH4000]-160)/6);
#endif
#if USE_OW
// RoBue:
// Zurücksetzen der Min/Max-Werte um 00:00 Uhr einschalten
if (( hh == 00 )&&( mm == 00 )) {
minmax = 1;
}
#endif
// ******************************************************************
// RoBue:
// 1-Wire-Temperatursensoren (DS18B20) abfragen
// ******************************************************************
#if USE_OW
uint8_t i = 0;
uint8_t subzero, cel, cel_frac_bits;
uint8_t tempID[OW_ROMCODE_SIZE];
// Messen bei ss=5,15,25,35,45,55
if ( ss%10 == 5 ) {
// Messen?
if ( messen == 1 ) {
// RoBue Anmerkung:
// Hiermit werden ALLE Sensoren zum Messen aufgefordert.
// Aufforderung nur bestimmter Sensoren:
// "NULL" durch "tempID" ersetzen
// RoBue Testausgabe UART:
// usart_write("Starte Messvorgang ...\r\n");
DS18X20_start_meas( DS18X20_POWER_PARASITE, NULL );
// Kein Messen mehr bis ss=5,15,25,35,45,55
messen = 0;
// Jetzt kann ausgelesen werden
auslesen = 1;
} // -> if messen
} // -> if ss
// Auslesen bei ss=8,18,28,38,48,58
if ( ss%10 == 8 ) {
// Auslesen?
if ( auslesen == 1 ) {
// (erste) ID ins RAM holen
memcpy_P(tempID,DS18B20IDs[0],OW_ROMCODE_SIZE);
while ( tempID[0] != 0 ) {
//while ( tempID[0] == 0x10 ) {
// RoBue Anmerkung:
// Hiermit wird jeweils ein einzelner Sensor ausgelesen
// und die Temperatur in ow_array abgelegt.
// Achtung:
// Pro Sekunde können max. ca. 10 Sensoren ausgelesen werden!
if ( DS18X20_read_meas( tempID, &subzero,&cel, &cel_frac_bits) == DS18X20_OK ) {
ow_array[i] = DS18X20_temp_to_decicel(subzero, cel, cel_frac_bits);
// Minuswerte:
if ( subzero )
ow_array[i] *= (-1);
// min/max:
if ( minmax == 1 ) {
// Zurücksetzen der Min/Max_Werte 1x/Tag
// auf die gerade aktuellen Temperaturen
ow_array[i+MAXSENSORS] = ow_array[i];
ow_array[i+MAXSENSORS*2] = ow_array[i];
}
else {
// Abgleich der Temp. mit den gespeicherten Min/Max-Werten
if (ow_array[i] < ow_array[i+MAXSENSORS])
ow_array[i+MAXSENSORS] = ow_array[i];
if (ow_array[i] > ow_array[i+MAXSENSORS*2])
ow_array[i+MAXSENSORS*2] = ow_array[i];
}
//TWert = DS18X20_temp_to_decicel(subzero, cel, cel_frac_bits);
//ow_array[i] = TWert;
// RoBue:
// Testausgabe UART:
// usart_write("%2i:%2i:%2i: Temperatur: %3i Grad\r\n",hh,mm,ss,ow_array[i]/10);
} // -> if
else {
usart_write("\r\nCRC Error (lost connection?) ");
DS18X20_show_id_uart( tempID, OW_ROMCODE_SIZE );
} // -> else
// nächste ID ins RAM holen
memcpy_P(tempID,DS18B20IDs[++i],OW_ROMCODE_SIZE);
} // -> while
// RoBue:
// Temperatur auf LCD ausgeben (IP von Startausgabe (s.o.) wird Überschrieben)
#if USE_SER_LCD
lcd_print(1,0,"Tmp: ");
lcd_print(1,5,"%i C",ow_array[0]/10);
lcd_print(1,11,"%i C",ow_array[1]/10);
#endif
} // -> if auslesen
auslesen = 0; // Auslesen vorläufig abschalten
messen = 1; // Messen wieder ermöglichen
minmax = 0; // Min/Max-Werte vergleichen
} // -> if ss
#endif
// **********************************************************
// RoBue:
// Schalten der Ports (PORTC) durch bestimmte Bedingungen
// - Temperatur (1-Wire -> PORTA7) mit/ohne Lüftungsautomatik
// - digital, analog (-> PORTA0-6)
// - Zeit
// **********************************************************
// Automatik eingeschaltet?
if ( var_array[9] == 1 ) {
if ( ss%10 == 1 ) {
schalten = 1;
}
// Abfrage bei ss =0,10,20,30,40,50
if ( ss%10 == 0 ) {
if ( schalten == 1 ) {
// RoBue Testausgabe UART:
// usart_write("%2i:%2i: Schaltfunktionen testen ...\r\n",hh,mm);
// PORTC0:
// Über Temperatur: var_array[10] - Sensor0
if (( ow_array[0]/10 < var_array[10] ) || ( ow_array[0] < 0 )) {
PORTC |= (1 << PC0); // ein
//
// Über Temperatur: var_array[10] - Sensor0 - PORTA0
// if ((PINA&0b00000001) == 0 ) { // PORTA0: Fenster geschlossen?
// if (( ow_array[0]/10 < var_array[10] ) || ( ow_array[0] < 0 )) {
// PORTC |= (1 << PC0); // ein
// }
// else {
// PORTC &= ~(1 << PC0); // aus
// }
}
else {
PORTC &= ~(1 << PC0); // aus
}
// PORTC1:
// Über Temperatur: var_array[11] - Sensor1
// if (( ow_array[1]/10 < var_array[11] ) || ( ow_array[1] < 0 )) {
// PORTC |= (1 << PC1); // ein
//
// Über Temperatur: var_array[11] - Sensor1 - PORTA1
if ((PINA&0b00000010) == 0 ) { // PORTA1: Fenster geschlossen?
if (( ow_array[1]/10 < var_array[11] ) || ( ow_array[1] < 0 )) {
PORTC |= (1 << PC1); // ein
}
else {
PORTC &= ~(1 << PC1); // aus
}
}
else {
PORTC &= ~(1 << PC1); // aus
}
// PORTC2:
// Über Temperatur: var_array[12] - Sensor2
// if (( ow_array[2]/10 < var_array[12] ) || ( ow_array[2] < 0 )) {
// PORTC |= (1 << PC2); // ein
// Über Temperatur: var_array[12] - Sensor2 - PORTA2
// if ((PINA&0b00000100) == 0 ) { // PORTA2: Fenster geschlossen?
// if (( ow_array[2]/10 < var_array[12] ) || ( ow_array[2] < 0 )) {
// PORTC |= (1 << PC2); // ein
// }
// else {
// PORTC &= ~(1 << PC2); // aus
// }
//}
//else {
// PORTC &= ~(1 << PC2); // aus
//}
// PORTC4:
// Über 2 Temperaturen (Differenz)
// var_array[13] - Sensor3 Vorlauf, Sensor4 Ruecklauf
//
// z.B. Zirkulationspumpe für Warmwasser
// Achtung: Temp. von Sensor3 MUSS höher/gleich sein als Temp. von Sensor4
// Ansonsten laeuft die Pumpe immer
//
if ( (ow_array[3]/10 - ow_array[4]/10) >= var_array[14] ) {
PORTC |= (1 << PC4); // ein
}
else {
PORTC &= ~(1 << PC4); // aus
}
// PORTC6:
// Über Analogwert:
if ( var_array[6] > var_array[18] ) {
PORTC |= (1 << PC6); // ein
}
else {
PORTC &= ~(1 << PC6); // aus
}
// PORTC7:
// Über Zeit: ein/aus
if ( hh == var_array[20] ) {
if ( mm == var_array[21] ) {
PORTC |= (1 << PC7); // ein
}
}
if ( hh == var_array[22] ) {
if ( mm == var_array[23] ) {
PORTC &= ~(1 << PC7); // aus
}
}
schalten = 0; // Vorerst nicht mehr schalten
} // -> schalten == 1
} // -> if ss == ...
} // -> if var_array == 1
//Wetterdaten empfangen (Testphase)
#if GET_WEATHER
http_request ();
#endif
//Empfang von Zeitinformationen
#if USE_NTP
if(!ntp_timer){
ntp_timer = NTP_REFRESH;
ntp_request();
}
#endif //USE_NTP
//Versand von E-Mails
#if USE_MAIL
if (mail_enable == 1)
{
mail_enable = 0;
mail_send();
}
#endif //USE_MAIL
//Rechner im Netzwerk aufwecken
#if USE_WOL
if (wol_enable == 1)
{
wol_enable = 0;
wol_request();
}
#endif //USE_WOL
//USART Daten für Telnetanwendung?
telnetd_send_data();
}
return(0);
}
int loadelf_load(char *ch, struct elf_image *img)
{
unsigned long i, j;
struct elf_hdr *ElfHdr = 0; /*elf header struct */
unsigned long p_e_entry; /* elf progamme start address */
unsigned long p_e_phoff; /* Start of program headers */
unsigned short p_e_phentsize; /* Size of program headers */
unsigned short p_e_phnum; /* number of program headers */
struct elf_phdr *ElfPhdr = 0; /* ELF program Header */
struct elf_phdr *phdr = 0; /* ELF program Header */
unsigned char *lptmp; /* load temp address */
WORD brs;
unsigned long br;
FRESULT res;
/* Need to load portions of the ELF file to read metadata. Where
should it be loaded? Choose high area of DDR.
TODO: Read metadata into SRAM (stack?) instead?
*/
unsigned char *readbuf = (unsigned char *)0x13000000;
img->entry = 0;
img->min_addr = (void *) 0xFFFFFFF0;
img->max_addr = (void *) 0;
/* ELF loader */
/* mount the volume */
lcd_clear();
lcd_print(0, "Pgm load");
lcd_print(1, ch);
res = pf_open(ch);
if (res != FR_OK) {
putstr("ELF not found\n");
lcd_print(1, "not fnd ");
return -1;
}
char str[32]; str[0] = '\0';
_strcat(str, "Open "); _strcat(str, ch); _strcat(str, " OK");
_strcat(str, "\n");
putstr(str);
res = pf_lseek(0);
if (res != FR_OK)
return -1;
/* read ELF header length */
pf_lseek(40); //ELF header length stored at 40
res = pf_read(readbuf, 2, &brs);
/*read elf magic number */
pf_lseek(0);
res = pf_read(readbuf, ((readbuf[0]) << 8) + readbuf[1], &brs);
if (res != FR_OK)
return -1;
ElfHdr = (struct elf_hdr *)readbuf;
if ((ElfHdr->e_ident[0] != 0x7f) || (ElfHdr->e_ident[1] != 'E')
|| (ElfHdr->e_ident[2] != 'L') || (ElfHdr->e_ident[3] != 'F'))
return -1;
if (ElfHdr->e_type != ET_EXEC)
return -1;
/* ELF executable programe running start address */
p_e_entry = ElfHdr->e_entry;
p_e_phoff = ElfHdr->e_phoff; /*Start of program headers */
p_e_phentsize = ElfHdr->e_phentsize; /*size of program headers */
p_e_phnum = ElfHdr->e_phnum; /*number of program headers */
/* relocate readbuf to avoid conflict */
/*
Don't need to play these games as readbuf is now high in DDR
memory.
if(p_e_entry <= p_e_phentsize + (unsigned)readbuf) {
if((p_e_entry - p_e_phentsize) > 0x100000200)
readbuf = (unsigned char*)(p_e_entry - p_e_phentsize - 256);
else
readbuf = (unsigned char*)0x12a00000;
}*/
lcd_print(1, "Loading");
/* load all program headers */
pf_lseek(p_e_phoff);
res = pf_read_long(readbuf, p_e_phentsize * p_e_phnum, &br);
if (res != FR_OK)
return -1;
/* calculate total in memory size for progress indicator */
unsigned long mem_total = 0;
unsigned long mem_progress = 0;
ElfPhdr = (struct elf_phdr *)readbuf;
for (i = 0; i < p_e_phnum; i++) {
if (ElfPhdr[i].p_type == PT_LOAD) {
mem_total += ElfPhdr[i].p_memsz;
}
}
/* copy segment section */
for (i = 0; i < p_e_phnum; i++) { /* number of programme header */
phdr = ElfPhdr + i;
if (phdr->p_type == PT_LOAD) { /* load to running address */
/* loading */
lptmp = (unsigned char *)phdr->p_paddr; /* assigned physAddr or virtAddr ? */
/* update min and max addresses */
if (img->min_addr > (void*) lptmp) {
img->min_addr = lptmp;
}
if (img->max_addr < (void*)(lptmp + phdr->p_memsz)) {
img->max_addr = lptmp + phdr->p_memsz;
}
if (phdr->p_filesz > 0) {
pf_lseek(phdr->p_offset);
res = read_progress(lptmp, phdr->p_filesz, &mem_progress, mem_total);
if (res != FR_OK)
return -1;
}
/* clean any memory that wasn't loaded */
if (phdr->p_filesz < phdr->p_memsz) {
lptmp += phdr->p_filesz;
for (j = phdr->p_filesz; j < phdr->p_memsz; j++) { /* clean memory to 0 */
/* TODO: align and write zero ints instead of zero chars */
*lptmp++ = 0;
}
mem_progress += phdr->p_memsz - phdr->p_filesz;
report_load_progress(mem_progress, mem_total);
}
#ifdef DEBUG
printf("loading Segment section No: %lu PhysAdd @ 0x%lX,FileSize=%lu,MemSize=%lu\n", i, phdr->p_paddr, phdr->p_filesz, phdr->p_memsz);
#endif
}
}
img->entry = (void *) p_e_entry;
return 0;
} /* ELF loader */
int main(int argc, char** argv)
{
int g, rep;
setup_io();
signal(SIGINT, intHandler);
debug("setting up io pins");
INP_GPIO(17);
OUT_GPIO(17);
//init_pwm();
int glow = 0;
int increment = 4;
while (0) {
glow += increment;
increment++;
if (glow > 1024 || glow < 0) {
glow -= increment;
increment = -increment;
}
//if (GPIO_LEVEL(8)) {
GPIO_SET = 1 << 17;
PWM_DATA = glow;
//} else {
// GPIO_CLR = 1 << 7;
//}
usleep(30000);
}
debug("initializing lcd");
init_lcd();
debug("writing to lcd");
lcd_print("Raspberry Pi :)", 1);
time_t t = time(NULL);
struct tm cur_time = *localtime(&t);
char time_str[19];
while (1) {
t = time(NULL);
cur_time = *localtime(&t);
strftime(time_str, 18, "%a %I:%M:%S %p", &cur_time);
lcd_print(time_str, 2);
sleep(1);
}
return 0;
for (rep=0; rep<10; rep++) {
for (g=7; g<=11; g++) {
GPIO_SET = 1<<g;
sleep(1);
}
//for (g=7; g<=11; g++) {
// GPIO_CLR = 1<<g;
// sleep(1);
//}
}
return 0;
}
int main(void) //Main program starts from here
{
double acc_Angle;
int gyro_Angle;
int filt_Angle;
unsigned int pwm_value;
init_adxl(); //Initialise accelerometer
init_gyro(); //Initialise gyroscope
init_devices1();
uart0_init(); //Initailize UART1 for serial communiaction
start_timer4(); //Timer for timing calculations
SetTunings(8.1,8,5);
lcd_print(1,1,kp*10,4);
lcd_print(1,6,ki*10,4);
lcd_print(1,11,kd*10,4);
while(1)
{
acc_Angle=0.1*acc_angle(); //Accelerometer angle
gyro_Angle=gyro_Rate(); //Anugular rate from Gyroscope
filt_Angle=comp_filter(acc_Angle,gyro_Angle); //Filtered angle after passing through Complementary filter
Input=filt_Angle; //Input for error calculation of PID
Compute(); //Calling PID
if (Output>0) //Mapping PID output to velocity of motors
{
pwm_value = (Output+THRESHOLD);
if(pwm_value>=255)
{
pwm_value=255;
}
set_PWM_value(pwm_value);
forward();
}
else if(Output<0)
{
pwm_value = (-Output)+THRESHOLD;
if(pwm_value>=255)
{
pwm_value=255;
}
set_PWM_value(pwm_value);
back();
}
else if(Input==0)
{
stop();
}
//_delay_ms(20);
UDR0=0xFF;
_delay_ms(1);
UDR0=(uint8_t)(filt_Angle+100);
_delay_ms(1);
uint8_t op=(Output/2)+127;
UDR0=op;
}
}
/*
* loop()
* Main program loop. Checks if connection succeeded then displays and updates time.
* @return bool True if error occured.
*/
static ICACHE_FLASH_ATTR
bool loop()
{
if(!init_done)
{
uint8_t status = wifi_station_get_connect_status();
switch(status)
{
case STATION_IDLE:
case STATION_CONNECTING:
return false;
case STATION_WRONG_PASSWORD:
case STATION_NO_AP_FOUND:
case STATION_CONNECT_FAIL:
lcd_clear();
lcd_print("Error: ");
lcd_set_cursor(0, 1);
switch(status)
{
case STATION_WRONG_PASSWORD:
lcd_print("Wrong password.");
return true;
case STATION_NO_AP_FOUND:
lcd_print("No AP found.");
return true;
case STATION_CONNECT_FAIL:
lcd_print("Connect fail.");
return true;
}
return;
case STATION_GOT_IP:
break;
}
struct ip_info ip;
wifi_get_ip_info(STATION_IF, &ip);
if(ip.ip.addr == 0)
return false; // no error
lcd_print("\xFE\x01IP: ");
char buff[128];
os_sprintf(buff, IPSTR, IP2STR(&ip.ip));
lcd_print(buff);
os_delay_us(1000000);
lcd_clear();
time_request();
init_done = true;
display_time();
return;
}
bool update_time = display_time();
if(update_time)
{
// time_request();
}
}
//Main Function
int main(void)
{
unsigned int value;
init_devices();
lcd_set_4bit();
lcd_init();
int i=0;
DDRH=0b01110000; //led pin config
char got;
char send;
char block_i;
while(1)
{
sharp=ADC_Conversion(11);
value=Sharp_GP2D12_estimation(sharp);
got=data;
if(got=='U' || got=='B'){
block_i=got;
}
lcd_print(1,1,got,2);
lcd_print(2,1,send,2);
if(value>100 && value<150){
send='B';
UDR0=send;
}
else{
send='U';
UDR0=send;
}
if(block_i=='U' && send=='B')
{
PORTH=0;
}
else if(block_i=='B' && send=='B')
{
PORTH=0b00100000;
}
else if(got=='R'){
PORTH=0b00010000;
got="";
}
else if(got=='G'){
PORTH=0b01000000;
got="";
}
else if(got=='T'){
PORTH=0;
got="";
}
}
}
int main3(void)
{
//test_lcd();
//return 0;
//test_adc();
/* Initialize system */
//SystemInit();
//TM_RCC_InitSystem();
//HAL_Init();
TM_DELAY_Init();
//GPIO_setup();
int i = 0;
#ifdef DISCOVERY
// GPIO_SetBits(GPIOD, GPIO_Pin_12); //Подаем «1» на PD12
//main2(); //return 0;
//GPIO_SetBits(GPIOD, GPIO_Pin_13); //Подаем «1» на PD12
while(1)
{
//Если кнопка нажата, то…
if (GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0)==1)
{
char sz[32];
sprintf(sz, "%d", i);
lcd_print(sz, 0);
//delay(1000);
int tm = 500;
GPIO_SetBits(GPIOD, GPIO_Pin_12); //Подаем «1» на PD12
delay(tm); //Функция задержки
GPIO_SetBits(GPIOD, GPIO_Pin_13); //Подаем «1» на PD13
delay(tm);
GPIO_SetBits(GPIOD, GPIO_Pin_14); //Подаем «1» на PD14
delay(tm);
//GPIO_SetBits(GPIOD, GPIO_Pin_15); //Подаем «1» на PD15
delay(tm);
GPIO_ResetBits(GPIOD, GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14/*|GPIO_Pin_15*/); //Сбрасываем все пины в «0»
delay(tm);
}
//delay(1000); // ~3sec
delay(100); // ~.3sec
if(i % 2 == 0)
GPIO_SetBits(GPIOD, GPIO_Pin_15); //Подаем «1» на PD15
else
GPIO_ResetBits(GPIOD, GPIO_Pin_15); //Подаем «1» на PD15
i++;
}
return 0;
#endif
//Если кнопка нажата, то…
/*
GPIO_setup();
button_setup();
GPIO_ResetBits(GPIOD, GPIO_Pin_12 | GPIO_Pin_13 |GPIO_Pin_14 |GPIO_Pin_15 );
initTimer();
startTimer(1000);
while(1)
{
}
*/
set_port(GPIOA,GPIO_Pin_5); // shotin
set_port(GPIOC, GPIO_Pin_5); // shotout
set_port(GPIOA, GPIO_Pin_3); // switch 1
reset_port(GPIOC, GPIO_Pin_13); // left
reset_port(GPIOC, GPIO_Pin_14); // right
reset_port(GPIOA, GPIO_Pin_4); // switch 2
while(1)
{
if (get_port(GPIOC,GPIO_Pin_13)==1) // left
{
lcd_print("left", 0);
set_port(GPIOA,GPIO_Pin_3); // sw1
}
else
reset_port(GPIOA,GPIO_Pin_3);
if (get_port(GPIOC,GPIO_Pin_14)==1) // right
{
set_port(GPIOA, GPIO_Pin_4); // switch 2
lcd_print("right", 0);
}
else
reset_port(GPIOA, GPIO_Pin_4);
if(i % 2 == 0)
set_port(GPIOC, GPIO_Pin_5); // shotout
else
reset_port(GPIOC, GPIO_Pin_5);
Delayms(3);
i++;
}
return 0;
}
//*******************************************************************************************
//
// Function : key_process
// Description : Process all key code from get_key_code function
//
//*******************************************************************************************
void menu_process ( void )
{
static BYTE key_hold_count=0, key_hold_step_delay=0;
BYTE rxtx_buffer[MAX_RXTX_BUFFER];
BYTE key_code, temp;
static BYTE backlight_seccount=250;
// get switch value from port
key_code = SW_PIN & ( _BV( SW_DW ) | _BV( SW_UP ) | _BV( SW_EXIT ) | _BV( SW_MENU ) );
// Check key press?
if ( key_code == ( _BV( SW_DW ) | _BV( SW_UP ) | _BV( SW_EXIT ) | _BV( SW_MENU ) ) )
{
flag1.bits.key_is_executed = 0;
flag2.bits.key_hold = 0;
key_hold_count = 0;
key_hold_step_delay = 0;
if(screen_timeout==0){
LCD_BL_PORT |= _BV( LCD_BL_PIN );
return;
}
// lcd backlight control
// lcd backlight off after key is unpress ( 30 seconds)
if ( timeout_cur_screen )
{
if ( -- backlight_seccount> 250 )
{
backlight_seccount = 250;
if ( --timeout_cur_screen == 0 )
{
timeout_cur_screen = 0;
// lcd backlight off
LCD_BL_PORT &= ~_BV( LCD_BL_PIN );
}
}
}
return;
}
// lcd backlight on
// and hold-on as screen_timeout seconds
timeout_cur_screen = screen_timeout;
LCD_BL_PORT |= _BV( LCD_BL_PIN );
// check hold key
if ( ++key_hold_count == 200 )
{
key_hold_count = 0;
flag2.bits.key_hold = 1;
}
if ( flag2.bits.key_hold )
{
if ( ++key_hold_step_delay == 30 )
{
key_hold_step_delay = 0;
if ( key_code == ((~_BV ( SW_UP ) ) & 0xf0) )
{
key_up_process ();
}
// if down key is pressed
else if ( key_code == ((~_BV ( SW_DW ) ) & 0xf0) )
{
key_dw_process ();
}
display_menu();
}
}
// key code already executed
if ( flag1.bits.key_is_executed )
return;
// check key code, what is key pressed?
// if menu key is pressed
if ( key_code == ((~_BV ( SW_MENU ) ) & 0xf0) )
{
// enter to main menu
if( menu_index == 0 )
{
setting_cursor = 0;
menu_index = 1;
submenu_index = 1;
}
// enter to submenu
else if( menu_index == 1 )
{
menu_stack = menu_index;
submenu_stack = submenu_index;
menu_index = submenu_index + menu_index;
submenu_index = 1;
}
// ping server
else if ( menu_index == 5 )
{
// Show on lcd first line
lcd_putc( '\f' );
lcd_print ( (BYTE *)menu_list[ 4 ] );
lcd_putc( '\n' );
if ( icmp_ping ( (BYTE*)rxtx_buffer, (BYTE*)&server_mac, (BYTE*)&server_ip ) )
{
lcd_print_p ( PSTR ( "Ping OK." ) );
}
else
{
lcd_print_p ( PSTR ( "Not found." ) );
}
flag1.bits.lcd_busy = 1;
menu_index = 0;
submenu_index = 0;
flag1.bits.key_is_executed = 1;
return;
}
// change cursor setting on each menu
else
{
temp = pgm_read_byte ( (PGM_P)(setting_cursor_max + menu_index - 2) );
if ( ++setting_cursor == temp )
setting_cursor = 0;
}
}
// if exit key is pressed
else if ( key_code == ((~_BV ( SW_EXIT ) ) & 0xf0) )
{
setting_cursor = 0;
if(menu_index > 1)
{
menu_index = menu_stack;
submenu_index = submenu_stack;
}
else
{
menu_index = 0;
submenu_index = 0;
}
}
// if up key is pressed
else if ( key_code == ((~_BV ( SW_UP ) ) & 0xf0) )
{
key_up_process ();
}
// if down key is pressed
else if ( key_code == ((~_BV ( SW_DW ) ) & 0xf0) )
{
key_dw_process ();
}
// display menu information on LCD
display_menu();
flag1.bits.key_is_executed = 1;
flag1.bits.lcd_busy = 0;
}
void lcd_print_time(byte line, rtc_time *time, temp_value temp)
{
snprintf(lcd_line, LINE_WIDTH, "%.2d:%.2d:%.2d%s %d.%dC%s", time->hour, time->minute, time->second, rtc_period[time->period], temp.temp, temp.point, blank_line);
lcd_print(line, lcd_line);
}
/**
* Main Function
*/
int main()
{
init_devices();
lcd_set_4bit();
lcd_init();
while(1)
{
Left_white_line = ADC_Conversion(3); ///< Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2);///< Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); ///< Getting data of Right WL Sensor
Front_Sharp_Sensor = ADC_Conversion(11);///< Getting data of Front Sharp sensor
Front_IR_Sensor = ADC_Conversion(6); ///< Getting data of Front IR sensor
flag1=0;
flag2=0;
print_sensor(1,1,3); ///< Prints value of White Line Sensor1
print_sensor(1,5,2); ///< Prints Value of White Line Sensor2
print_sensor(1,9,1); ///< Prints Value of White Line Sensor3
lcd_print(1, 13, timer_flag, 4);
if((Center_white_line<0x28))
{
flag1=1;
forward();
velocity(RegSpeed,RegSpeed);
}
if((Left_white_line>0x28) && (flag1==0))
{
flag1=1;
forward();
velocity(100,50);
}
if((Right_white_line>0x28) && (flag1==0))
{
flag1=1;
forward();
velocity(50,100);
}
if((Center_white_line>0x28) && (Left_white_line>0x28) && (Right_white_line>0x28))
{
forward();
velocity(RegSpeed,RegSpeed);
if (timer_flag == 0) ///< vehicle at first black patch
{
timer_flag=1;
while(StartTheBot==0)
{
stop();
}
if (StartTheBot == 1)
{
forward();
velocity(RegSpeed,RegSpeed);
TCCR1B = 0x01; ///< Timer 1 start witrh no prescaler
while(Center_white_line>0x28 && Left_white_line>0x28 && Right_white_line>0x28)//wait till the time the entire red line is crossed.
{
Left_white_line = ADC_Conversion(3); ///< Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); ///< Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); ///< Getting data of Right WL Sensor
}
timer_flag=2;
}
}
else if (timer_flag == 2) ///< vehicle at second black patch
{
TCCR1B = 0x00; ///< Timer 1 stop
timer_flag=3;
lcd_print(2, 1, TCNT1H, 4);
lcd_print(2, 6, TCNT1L, 4);
lcd_print(2, 11, speed_cnt, 5);
speed = 27/ (((unsigned int) speed_cnt * 65536 * 90.9E-9) + ((unsigned int) TCNT1H * 256 * 90.9E-9) + ((unsigned int) TCNT1L * 90.9E-9));
f=1;
speed_int= speed;
speed_dec=(speed-speed_int)*100;
}
else if (timer_flag ==3) ///< vehicle at third black patch
{
timer_flag=4;
while (vehicle_stop==0) ///< Capturing photo in progress
{
stop();
}
if(vehicle_stop==1) ///< photograph taken. Vehicle can go ahead now
{
forward();
}
}
}
if((Front_Sharp_Sensor>0x80)|| (Front_IR_Sensor<0xF0)) ///< Obstacle detection
{
flag2=1;
stop();
}
}
}
void lcd_print_date(byte line, rtc_time *time)
{
snprintf(lcd_line, LINE_WIDTH, "%s %.2d-%.2d-%d%s", rtc_days[time->w_day], time->day, time->month, time->year, blank_line);
lcd_print(line, lcd_line);
}
void lcd_testprint0 (void )
{
lcd_print ( "XXXXXXXXXXXXXXXX" , 0 , 0 ) ;
lcd_print ( "XXXXXXXXXXXXXXXX" , 0 , 1 ) ;
lcd_print ( "XXXXXXXXXXXXXXXX" , 0 , 2 ) ;
lcd_print ( "XXXXXXXXXXXXXXXX" , 0 , 3 ) ;
lcd_print ( "XXXXXXXXXXXXXXXX" , 0 , 4 ) ;
lcd_print ( "XXXXXXXXXXXXXXXX" , 0 , 5 ) ;
lcd_print ( "abc123def456ghi7" , 0 , 0 ) ;
lcd_print ( "abc123def456ghi7" , 1 , 1 ) ;
lcd_print ( "abc123def456ghi7" , 2 , 2 ) ;
lcd_print ( "abc123def456ghi7" , 15 , 3 ) ;
lcd_print ( "abc123def456ghi7" , 18 , 5 ) ;
lcd_update ();
}
/*
? *
? * Function Name: <Print_White_Line_Sensors()>
? * Example Call: <Print_White_Line_Sensors()>
? *
? */
void Print_White_Line_Sensors()
{
lcd_print (1,1, Left_white_line, 4);
lcd_print (1,6, Center_white_line, 4);
lcd_print (1,11, Right_white_line, 4);
}
void em_Event( em_event_t evnt )
{
//signed char (*p_func_s)( signed char);
//unsigned char (*p_func_u)( unsigned char);
char v_char;
char vis_buf[5];
if ( selected == FALSE) {
// ######## UNSELECTED #################
switch(evnt) {
//==================================
case em_press:
switch(p_Act_item->type) {
// -------------------------
case submenu:
selected = FALSE;
active_pos = 0;
p_Itm_entry = p_Act_item->item; // Entry point to submenu
p_Act_item = p_Itm_entry; // place user pointer to first line
break;
// -------------------------
case schar:
// p_func_s = p_Act_item->item;
// v_char = (*p_func_s)(0);
v_char = ((signed char (*)(signed char))p_Act_item->item)(0);
xformat( vis_buf, 0, v_char);
lcd_print( 9, active_pos, vis_buf );
selected = TRUE;
break;
// -------------------------
case uchar:
lcd_pchar(1, 5, 'U');
// p_func_s = p_Act_item->item;
// v_char = (*p_func_u)(0);
v_char = ((unsigned char (*)(signed char))p_Act_item->item)(0);
xformat( vis_buf, 1, v_char);
lcd_print( 9, active_pos, vis_buf );
selected = TRUE;
break;
// -------------------------
// case str:
// break;
}
break;
//==================================
case em_up:
active_pos--;
break;
//==================================
case em_down:
active_pos++;
break;
//==================================
}
} else {
// ######## SELECTED ###################
switch(evnt) {
//==================================
case em_press:
switch(p_Act_item->type) {
// -------------------------
case submenu:
selected = FALSE;
active_pos = 0;
p_Itm_entry = p_Act_item->item; // Entry point to submenu
p_Act_item = p_Itm_entry; // place user pointer to first line
break;
// -------------------------
case schar:
// p_func_s = p_Act_item->item;
// v_char = (*p_func_s)(0);
v_char = ((signed char (*)(signed char))p_Act_item->item)(0);
xformat( vis_buf, 0, v_char);
lcd_print( 9, active_pos, vis_buf );
selected = FALSE;
break;
// -------------------------
case uchar:
lcd_pchar(4, 5, '*');
// p_func_s = p_Act_item->item;
// v_char = (*p_func_u)(0);
v_char = ((unsigned char (*)(signed char))p_Act_item->item)(0);
xformat( vis_buf, 1, v_char);
lcd_print( 9, active_pos, vis_buf );
lcd_pchar(4, 6, '=');
selected = FALSE;
break;
// -------------------------
// case str:
// break;
}
break;
//==================================
case em_up:
switch(p_Act_item->type) {
// -------------------------
case submenu:
break;
// -------------------------
case schar:
// p_func_s = p_Act_item->item;
// v_char = (*p_func_s)(1);
v_char = ((signed char (*)(signed char))p_Act_item->item)(1);
xformat( vis_buf, 0, v_char);
lcd_print( 9, active_pos, vis_buf );
break;
// -------------------------
case uchar:
lcd_pchar(1, 5, '<');
//p_func_s = p_Act_item->item;
//v_char = (*p_func_u)(1);
v_char = ((unsigned char (*)(signed char))p_Act_item->item)(1);
xformat( vis_buf, 0, v_char);
lcd_print( 9, active_pos, vis_buf );
break;
}
break;
//==================================
case em_down:
switch(p_Act_item->type) {
// -------------------------
case submenu:
break;
// -------------------------
case schar:
// p_func_s = p_Act_item->item;
// v_char = (*p_func_s)(-1);
v_char = ((signed char (*)(signed char))p_Act_item->item)(-1);
xformat( vis_buf, 0, v_char);
lcd_print( 9, active_pos, vis_buf );
break;
// -------------------------
case uchar:
lcd_pchar(1, 5, '>');
// p_func_s = p_Act_item->item;
// v_char = (*p_func_u)(-1);
v_char = ((unsigned char (*)(signed char))p_Act_item->item)(-1);
xformat( vis_buf, 0, v_char);
lcd_print( 9, active_pos, vis_buf );
break;
}
break;
//==================================
}
}
em_Show();
}
//***************************************************
//***************************************************
void LCD_service::lcd_print(const int x, const int y,
const char* str_msg)
{
lcd_setCursor(x, y);
lcd_print(str_msg);
}
//Main Function
int main(void)
{
unsigned char flag ;
init_devices();
lcd_set_4bit();
lcd_init();
velocity(VELOCITY_MAX,VELOCITY_MAX); // Set the speed to max velocity
forward(); // start to move froward
unsigned char lch=0;
unsigned char rch=0;
unsigned char cch=0;
int ler=0;
int rer=0;
float thd=10;
int per=0;
int thdh=80;
int thdl=20;
float kp=10;
int kd=50;
int rl=0,old_rl=0;
int PWM_ratio_1 = 10;
int PWM_ratio_2 = 25;
unsigned char old_ler=0;
unsigned char old_rer=0;
unsigned char old_per=100;
while(1)
{
lch=ADC_Conversion(3);
rch=ADC_Conversion(5);
cch=ADC_Conversion(4);
ler=lch-cch;
rer=rch-cch;
per=lch-rch;
rl=rch-lch;
print_sensor(1,1,3); //Prints value of White Line Sensor Left
print_sensor(1,5,4); //Prints value of White Line Sensor Center
print_sensor(1,9,5); //Prints value of White Line Sensor Right
if(rl>0)
{
thd = 120 - (kp*rl+kd*(rl-old_rl));
}
else
{
thd = -120 - (kp*rl+kd*(rl-old_rl));
}
lcd_print (2,1,abs(rl),3);
lcd_print (2,5,(unsigned char)abs(thd),3);
if(rl>(-thd) && rl<(thd))
{
forward();
//velocity(255,255);
}
else
{
if(rl>0)
{
if(rl<thd+30)
soft_right();
//velocity(min(255,rl*PWM_ratio_1),0);
else
right();
//velocity(min(255,rl*PWM_ratio_2),0);
}
else
{
if(rl>-thd-30)
soft_left();
//velocity(0,min(255,-rl*PWM_ratio_1));
else
left();
//velocity(0,min(255,-rl*PWM_ratio_2));
}
}
old_rl=rl;
}
}
//Main Function
int main()
{
unsigned char flag ;
init_devices();
lcd_set_4bit();
lcd_init();
velocity(VELOCITY_MAX,VELOCITY_MAX); // Set the speed to max velocity
lcd_print (2,1,VELOCITY_MAX,3);
lcd_print (2,5,VELOCITY_MAX,3);
forward(); // start to move froward
while(1)
{
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(4); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(5); //Getting data of Right WL Sensor
print_sensor(1,1,3); //Prints value of White Line Sensor Left
print_sensor(1,5,4); //Prints value of White Line Sensor Center
print_sensor(1,9,5); //Prints value of White Line Sensor Right
flag=0;
if(Center_white_line<THRESHOLD) // Is middle Whiteline is within threshold limit
{
USART_Transmit(0x32);
flag=1;
velocity(VELOCITY_MAX,VELOCITY_MAX); // Run robot at max velocity
lcd_print (2,1,VELOCITY_MAX,3);
lcd_print (2,5,VELOCITY_MAX,3);
}
if((Left_white_line>THRESHOLD) && (flag==0)) // Is left Whiteline is not within threshold limit
//if((Left_white_line>THRESHOLD) && (flag==0)) // Is left Whiteline is not within threshold limit
{
USART_Transmit(0x34);
flag=1;
velocity(VELOCITY_MAX,VELOCITY_MIN); // Run robot left wheel at max velocity and right wheel
lcd_print (2,1,VELOCITY_MAX,3); // at min velocity
lcd_print (2,5,VELOCITY_MIN,3);
/* velocity(VELOCITY_MIN,60); // Run robot right wheel at max velocity and left wheel
lcd_print (2,1,VELOCITY_MIN,3); // at min velocity
lcd_print (2,5,VELOCITY_MAX,3);*/
}
if((Right_white_line>THRESHOLD) && (flag==0)) // Is right Whiteline is not within threshold limit
//if((Right_white_line>THRESHOLD) && (flag==0)) // Is right Whiteline is not within threshold limit
{
USART_Transmit(0x36);
flag=1;
velocity(VELOCITY_MIN,VELOCITY_MAX); // Run robot right wheel at max velocity and left wheel
lcd_print (2,1,VELOCITY_MIN,3); // at min velocity
lcd_print (2,5,VELOCITY_MAX,3);
/* velocity(60,VELOCITY_MIN); // Run robot left wheel at max velocity and right wheel
lcd_print (2,1,VELOCITY_MAX,3); // at min velocity
lcd_print (2,5,VELOCITY_MIN,3);*/
}
if(Center_white_line>THRESHOLD && Left_white_line>THRESHOLD && Right_white_line>THRESHOLD && (flag == 0))
// if all Whiteline sensor are not within threshold limit
{
USART_Transmit(0x38);
flag=1;
velocity(VELOCITY_LOW,VELOCITY_LOW); // stop the robot
lcd_print (2,1,VELOCITY_LOW,3);
lcd_print (2,5,VELOCITY_LOW,3);
}
}
}
void Cursor_on(Cursor *cur){
lcd_locate(cur->relative.x, cur->relative.y);
lcd_setcolor(COL_BLUE);
lcd_print("@");
lcd_setcolor(COL_WHITE);
}
void main(void)
{
stop_watchdog();
set_clk_8MHz();
//interrupts_enable();
lcd_init();
uart_init();
/*
* Read a regiser (data[1]) and print register and value on the display
* */
unsigned char i2creceive[8];
i2c_master_init();
unsigned char data[1] = {0x6F};
lcd_goto(1,0);
lcd_print("R: ");
lcd_print(int2HEXcharArray(*data));
lcd_goto(2,0);
lcd_print("V: ");
i2c_writeByte(1,data,0x50);
__delay_cycles(300); // delay some time to get a better view on the signals
unsigned char rx_byte = i2c_receiveSingleByte(0x50);
__delay_cycles(300);
lcd_print(int2HEXcharArray(rx_byte));
/*
* receive 256 register values via UART and write them to the EEPROM (incrementing address from 0x00 to 0xFF)
* */
///*
unsigned char write_byte[2];
unsigned int write_counter = 0;
while(write_counter < 255) {
lcd_goto(2,0);
write_byte[0] = write_counter;
write_byte[1] = uart_rx_char();
lcd_print(int2HEXcharArray(write_byte[1]));
i2c_writeByte(2,write_byte,0x50);
write_counter = write_counter + 1;
}
//*/
/*
* Read out a register value and print it on the display to verify the write process
* */
lcd_goto(2,0);
lcd_print("V: ");
i2c_writeByte(1,data,0x50);
rx_byte = i2c_receiveSingleByte(0x50);
__delay_cycles(300);
lcd_print(int2HEXcharArray(rx_byte));
__delay_cycles(300);
for (;;)
{
}
}
void Print_LCD(void) {
lcd_clear();
lcd_print("Heading: %u\n", heading);
lcd_print("Altitude: %u\n", range);
lcd_print("Battery Voltage: %d", ADC_convert());
}
//-----------------------------------------------------------------------------
// Choose speed
//-----------------------------------------------------------------------------
//*************************MODIFIED NO AD!!!
void pick_speed(void){
lcd_clear();
lcd_print("To set speed, adjust pMeter now");
lcd_print("\n6 seconds to do so");
pause();
lcd_clear();
lcd_print("To set speed, adjust pMeter now");
lcd_print("\n5 seconds to do so");
pause();
lcd_clear();
lcd_print("To set speed, adjust pMeter now");
lcd_print("\n4 seconds to do so");
pause();
lcd_clear();
lcd_print("To set speed, adjust pMeter now");
lcd_print("\n3 seconds to do so");
pause();
lcd_clear();
lcd_print("To set speed, adjust pMeter now");
lcd_print("\n2 seconds to do so");
pause();
lcd_clear();
lcd_print("To set speed, adjust pMeter now");
lcd_print("\n1 seconds to do so");
pause();
}//end pick motor speed
int calcThresh()
{
int c[4],i;
red_read();
blue_read();
green_read();
for(i=0;i<4;i++)
c[i]=0;
for(i=0;i<4;i++)
{
//read();
if(red>green && red>blue)
if(c[0]>red)
{
if(c[3]<red)
c[3]=red;
}
else
{
if(c[3]<c[0])
c[3]=c[0];
c[0]=red;
}
else if(blue>green && red<blue)
if(c[1]>blue)
{
if(c[3]<blue)
c[3]=blue;
}
else
{
if(c[3]<c[1])
c[3]=c[1];
c[1]=blue;
}
else
if(c[2]>green)
{
if(c[3]<green)
c[3]=green;
}
else
{
if(c[3]<c[2])
c[3]=c[2];
c[2]=green;
}
}
int t=c[3];
lcd_cursor(1,1); //set the cursor on row 1, column 1
lcd_string("Threshold"); // Display "Blue Pulses" on LCD
t+=200;
lcd_print(2,1,t,5);
_delay_ms(10000);
lcd_wr_command(0x01); //Clear the LCD
lcd((char*)t);
return t;
}
void lcd_hexidecimal(unsigned char line, unsigned int value, char digits)
{
lcd_instruction(line);
lcd_print(value, 16, UNSIGNED_ZEROS, digits);
}