/*******************************************************************
* MAIN()
*******************************************************************/
int
main(void)
{
long lEEPROMRetStatus;
uint16_t i=0;
uint8_t halted_latch = 0;
// Set the clocking to run at 80 MHz from the PLL.
// (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't
// write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we
// could slow the clock down when writing to FLASH but then we need to find out how long
// it takes for the clock to stabilize. This is on at the bottom of my list of things to do
// for now)
SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
// Initialize the device pinout.
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ);
// Enable processor interrupts.
IntMasterEnable();
// Setup the UART's
my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// command_handler_init overwrites the baud rate. We still need to configure the pins though
my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// Enable the command handler
command_handler_init(); // We set the baud in here
// Start the timers
my_timer0_init();
my_timer1_init();
i2c_init();
motor_init();
qei_init();
gyro_init();
accel_init();
led_init();
//rc_radio_init();
//setupBluetooth();
// Initialize the EEPROM emulation region.
lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE);
if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14);
#if 0
// If ever we wanted to write some parameters to FLASH without the HMI
// we could do it here.
SoftEEPROMWriteDouble(kP_ID, 10.00);
SoftEEPROMWriteDouble(kI_ID, 10.00);
SoftEEPROMWriteDouble(kD_ID, 10.00);
SoftEEPROMWriteDouble(ANG_ID, 0.0);
SoftEEPROMWriteDouble(COMPC_ID, 0.99);
#endif
kP = SoftEEPROMReadDouble(kP_ID);
kI = SoftEEPROMReadDouble(kI_ID);
kD = SoftEEPROMReadDouble(kD_ID);
commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID);
COMP_C = SoftEEPROMReadDouble(COMPC_ID);
pid_init(kP, kI, kD, &pid_ang);
motor_controller_init(20, 100, 10, &mot_left);
motor_controller_init(20, 100, 10, &mot_right);
//pid_init(0.0, 0.0, 0.0, &pid_pos_left);
//pid_init(0.0, 0.0, 0.0, &pid_pos_right);
//UART0Send("Hello World!\n", 13);
// Tell the HMI what the initial parameters are.
print_params(1);
while(1)
{
delta_t = myTimerValueGet();
myTimerZero();
sum_delta_t += delta_t;
// Read our sensors
accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true);
gyro_get_y_cal(&gyro_y, false);
// Calculate the pitch angle with the accelerometer only
R = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang;
//accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0);
gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t);
// Kalman filter
//filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t));
filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang);
// Skip the rest of the process until the angle stabilizes
if(i < 250) { i++; continue; }
// Tell the HMI what's going on every 100ms
if(sum_delta_t >= 1000)
{
print_update(1);
print_debug(0);
//print_control_surfaces(0);
led_toggle();
//print_angle();
sum_delta_t = 0;
}
// See if the HMI has anything to say
command_handler();
//continue;
// If we are leaning more than +/- FALL_ANG deg off center it's hopeless.
// Turn off the motors in hopes of some damage control
if( abs(filtered_ang) > FALL_ANG )
{
if(halted_latch) continue;
stop_motors();
halted_latch = 1;
continue;
}
halted_latch = 0;
motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang);
motor_left = motor_right = motor_val;
drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain);
}
}
int main(void) {
volatile uint32_t valore = 0, i, blink = 0, contatore, lampeggio_led;
volatile int32_t arrot;
volatile int16_t val1 = 0, x, y, z;
distanza DIST;
//--------------------------//
///definizione strutture/////
//-------------------------//
//volatile double d = 1.9845637456;
gyro G;
accelerazione A;
cinematica CIN;
/// servono differenti PID, almeno uno per la rotazione ed uno per lo spostamento
/// per la rotazione sarebbero interessante usarne 2, uno per la ortazione soft ed uno per la rotazione
/// brusca.
pid CTRL[3], * pidPtr;
/// descrittore della sintassi dei comandi
syn_stat synSTATO;
/// modulo zigbee per telemetria
xbee XB;
/// pwm servi e motori
pwm PWM, pwmServi;
/// struttura del sensore di colore
colore COL;
/// sensore di temperatura ad infrarossi
temperatura TEMP;
TEMPER sensIR;
/// indormazioni sul sopravvissuto
survivor SUR;
//inizializzazione struttura per qei
qei QEI;
/// oggetto che riallinea il mezzo
allineamento AL;
/// disabilita le interruzioni
DI();
pidPtr = CTRL;
dPtr = &DIST;
TEMPptr = &TEMP;
CIN.Aptr = &A;
CIN.distPTR = &DIST;
CIN.vel = 0.0;
dati DATA;
//passaggio degli indirizzi delle strutture alla struttura generale
dati_a_struttura(&G, &DIST, &CIN, &COL, &TEMP, &SUR, &DATA);
/// commento per provare il merge su server remoto
/// setup di base
setupMCU();
/// imposta i parametri del PID
setupPID(CTRL);
/// imposta le UART
setupUART();
//inizializzo l'i2c
InitI2C0();
/// messaggio d'inizio
PRINT_WELCOME();
/// inizializza il giroscopio
initGyro(&G, Z_AXIS);
/// inizializza il timer 0 e genera un tick da 10 ms
initTimer0(10, &G);
/// inizializza il timer 1
initTimer1(100);
/// inizializza il contatore della persistenza del comando
synSTATO.tick = 0;
/// inizializza il pwm
pwmMotInit(&PWM);
// TODO: //pwmServoInit (&pwmServi);
/// inizializza l'adc e lo prepara a funzionare ad interruzioni.
initAdc(&DIST);
/// reset dell'automa di analisi della sintassi
resetAutoma(&synSTATO);
//servo = (pwm *) &pwmServi;
/// iniziailizzazione del lettore encoder
qei_init(&QEI);
/// abilita le interruzioni
EI();
/// attende che il sensore vada a regime
if (G.IsPresent == OK){
PRINTF("\nAzzeramento assi giroscopio\n");
while (blink < 70){
if (procCom == 1){
procCom = 0;
blink++;
}
}
blink = 0;
/// azzeramento degli assi
azzeraAssi(&G);
}
/// test della presenza del modulo zig-bee
/// il modulo zig-be si attiva con al sequnza '+++' e risponde con 'OK' (maiuscolo)
if (testXbee() == 0){
// ok;
XB.present = 1;
PRINTF("Modulo xbee presente.\n");
}
else{
XB.present = 0;
PRINTF("Modulo xbee non presente.\n");
}
pwm_power(&PWM);
contatore = 0;
//// inizializza l'accelrometro
//stato = writeI2CByte(CTRL_REG1_A, ODR1 + ODR0 + ZaxEN + YaxEN + XaxEN);
// scrivo nel registro 0x20 il valore 0x0F, cioe' banda minima, modulo on e assi on
/// sintassi: indirizzo slave, num parm, indirizzo reg, valore da scrivere
//I2CSend(ACCEL_ADDR, 2, CTRL_REG1_A, ODR1 + ODR0 + ZaxEN + YaxEN + XaxEN);
A.isPresent = testAccel();
if (A.isPresent)
impostaAccel(&A);
/// taratura sul sensore di luminosita'
whiteBal(&COL);
/// taratura del sensore di temepratura
taraturaTemp(&TEMP);
///
qei_test(&QEI);
/// task principale
while(1){
/// invia la risposta per i comandi di rotazione, quando sono stati eseguiti
if(pidPtr->rispondi == TRUE){
rispostaRotazione(pidPtr, &synSTATO);
pidPtr->rispondi = FALSE;
}
if (procCom == 1 ){
//UARTCharPutNonBlocking(UART1_BASE, 'c');
procCom = 0;
contatore++;
lampeggio_led++;
if(lampeggio_led >= 50)
{
lampeggio_led = 0;
if(DATA.surPtr->isSurvivor == TRUE )
{
if(HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GREEN_LED << 2))) != GREEN_LED )
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (RED_LED << 2))) = 0;
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GREEN_LED | RED_LED << 2))) ^= GREEN_LED | RED_LED;
}
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GREEN_LED << 2))) ^= GREEN_LED;
}
/* LETTURA DEL COMANDO */
/// restituisce l'indirizzo del PID da utilizzare nel successivo processo di calcolo
pidPtr = leggiComando(&synSTATO, CTRL, pidPtr, &DATA);
/* LETTURA SENSORI */
/// effettua i calcoli solo se il giroscopio e' presente
/// TODO: il PID viene calcolato ongi 10ms oppure ogni 20ms? Come è meglio?
/* misura gli encoder e calcola spostameti e velocità */
/* misura i sensori di distanza */
if (DIST.run == true)
/// TODO controllare se riesce a funzionare mentre legge le accelerazioni su I2C
ROM_ADCProcessorTrigger(ADC0_BASE, 0);
/// misura i dati forniti dall'accelerometro se disponibili
if(A.isPresent)
misuraAccelerazioni(&A);
/// le misure del giroscopio invece sono effettuate solo dall'apposito pid
/*if(G.IsPresent == OK)
if( contatore == 1){
/// ogni 10 ms effettua il calcolo del PID
contatore = 0;
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + (GPIO_PIN_0 << 2))) |= GPIO_PIN_0;
PID(&G, pidPtr, &PWM, &CIN);
setXPWM(&CTRL[1], &PWM);
procCom = 0;
HWREG(GPIO_PORTB_BASE + (GPIO_O_DATA + (GPIO_PIN_0 << 2))) &= ~GPIO_PIN_0;
blink++;
/// lampeggio del led con periodo di 2 s.
if (blink >= 100){
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + ((GPIO_PIN_2 | GPIO_PIN_1) << 2))) = 0;
HWREG(GPIO_PORTF_BASE + (GPIO_O_DATA + (GPIO_PIN_3 << 2))) ^= GPIO_PIN_3;
blink = 0;
}
///provvede ad integrare la misura della velcita' angolare ogni 10 ms
//misuraAngoli(&G);
//PRINTF("asse x: %d\t", G.pitch);
//PRINTF("\tasse y: %d\t", G.roll);
//PRINTF("\tasse z: %d\n", G.yaw);
//PRINTF("uscita PID: %d\n", C.uscita);
}*/
/* RISPOSTA AL COMANDO */
inviaSensore(&synSTATO, &DATA);
}
}
}
