int main(void) {
uchar i, j;
/* no pullups on USB and ISP pins */
//PORTD = 0;
//PORTB = 0;
/* all outputs except PD2 = INT0 */
/// LB - different pins on STK500 board
//DDRD = ~(1 << 2);
// make JTAG pins inputs with pullups
SET_JTAG_PULLUPS();
/* output SE0 for USB reset */
/// LB - different pins on STK500 board
//DDRB = ~0;
j = 0;
/* USB Reset by device only required on Watchdog Reset */
while (--j) {
i = 0;
/* delay >10ms for USB reset */
while (--i)
;
}
/* all USB and ISP pins inputs */
//DDRB = 0;
/// LB - LED pins are different from usbasp to sp duo - conflict: SP duo uses these for JTAG
/* all inputs except PC0, PC1 */
//DDRC = 0x03;
//PORTC = 0xfe;
SET_LED_OUTPUT();
LED_OFF();
/* init timer */
clockInit();
#ifdef UART_DEBUG
// init debug uart
setupUART();
TransmitString("\r\n\n***\r\nstarting up\r\n");
#endif
// USB Re-Enumeration
usbDeviceDisconnect();
while(--i){ // fake USB disconnect for > 250 ms
wdt_reset(); // if watchdog is active, reset it
_delay_ms(1); // library call -- has limited range
}
usbDeviceConnect();
/* main event loop */
usbInit();
sei();
for (;;) {
usbPoll();
}
return 0;
}
void setup()
{
setupSystemClock();
setup_system_tick(SYSTEM_TICK_FREQ);
setupUART();
#ifdef GPS
setupGPS();
#endif
I2C_Init();
FlashInit();
UpdateBoardVersion(false);
#ifdef OPTION_RC
RC_Init();
if(IsSSVConnected())
Battery_Init();
LED_Init();
TIMER_Init();
stabilizerInit();
#endif
#ifdef ABROBOT
ABRobotMotorInit();
#endif
nvtAHRSInit();
SensorsInit();
ChronographSet(ChronMain);
}
void BT_startAll() {
starting = 1;
initRN1();
_delay_cycles(48000000);
initRN2();
_delay_cycles(120000);
initRN3();
setupUART("115K");
_delay_cycles(360000);
starting = 0;
runSetCommands();
}
int main(void)
{
Board_Init();
SystemCoreClockUpdate();
Init_UART_PinMux();
Chip_UART_Init(LPC_USART0);
Board_LED_Set(0, false);
/* Allocate UART handle, setup UART parameters, and initialize UART
clocking */
setupUART();
/* Transmit the welcome message and instructions using the
putline function */
putLineUART("LP540xx USART API ROM polling Example\r\n");
putLineUART("String receive (no echo): "
"Enter a string and press enter to echo if back\r\n");
/* Get a string for the UART and echo it back to the caller. Data is NOT
echoed back via the UART using this function. */
getLineUART(recv_buf, sizeof(recv_buf));
recv_buf[sizeof(recv_buf) - 1] = '\0'; /* Safety */
if (strlen(recv_buf) == (sizeof(recv_buf) - 1)) {
putLineUART("**String was truncated, input data longer than "
"receive buffer***\r\n");
}
putLineUART(recv_buf);
/* Transmit the message for byte/character part of the exampel */
putLineUART("\r\nByte receive with echo: "
"Press a key to echo it back. Press ESC to exit\r");
/* Endless loop until ESC key is pressed */
recv_buf[0] = '\n';
while (recv_buf[0] != ESCKEY) {
/* Echo it back */
LPC_UARTD_API->uart_put_char(uartHandle, recv_buf[0]);
/* uart_get_char will block until a character is received */
recv_buf[0] = LPC_UARTD_API->uart_get_char(uartHandle);
}
/* Transmit the message for byte/character part of the exampel */
putLineUART("\r\nESC key received, exiting\r\n");
while (1) {
__WFI();
}
return 0;
}
void setup()
{
int i;
setupSystemClock();
setupUART();
setup_system_tick(SYSTEM_TICK_FREQ);
ID_Init(); // MO: Is it also system init?
GetID();
/* Init AHRS I2C */
/* ----Initialize I2C slave mode--- */
if(devNum == 0)
I2C_MS_Master_Init();
else
I2C_MS_Slave_Init();
//I2CWakeUpInit();
TIDMstFirstInitFIN = 0;
TIDMstInitFIN = 0;
TIDMstStage = 0;
TIDMstInitDevState = 1;
FlashInit();
// Setup function pointers for default sensor boards.
for(i=0; i<MAX_TID_DEV; i++)
{
if (pTidList[i])
SetDeviceFunction(pTidList[i], &(pfnDevFunc[i]));
}
// Call board init function
if (pTidList[devNum]->func.pfnSetup)
pTidList[devNum]->func.pfnSetup();
// Load TID feature from flash
GetFlashTID(&(pTidList[devNum]->Feature), devNum);
// Set Timer1 to (1000/value) in (Hz)
Timer1Init(1000 / (pTidList[devNum]->Feature.data1.value));
/* TID initialize */
SlvDataInit();
MstDataInit();
}
uint8_t runSetTempBaudRate() {
if(!bt_setbaudrate_start) {
BT_rst_MessageProgress();
command_received = 1;
bt_setbaudrate_start = 1;
DMA2_disable();
}
if(command_received) {
command_received = 0;
if(bt_setbaudrate_step == 0) {
bt_setbaudrate_step++;
writeCommand("$$$", "CMD\r\n");
return 0; //wait until response is received
}
// Connect
if(bt_setbaudrate_step == 1) {
bt_setbaudrate_step++;
sprintf(commandbuf, "U,%s,N\r", userBaudRate);
if(slowRate) {
//UCA1IE &= ~UCRXIE; //Disable USCI_A1 RX interrupt
writeCommandNoRsp(commandbuf);
DMA2_disable();
_delay_cycles(4800000);
DMA2SZ = 1;
DMA2_enable();
} else {
writeCommand(commandbuf, "AOK\r\n");
//__bis_SR_register(LPM3_bits + GIE); //wait until response is received
}
return 0;
}
if(bt_setbaudrate_step == 2) {
bt_setbaudrate_step = 0;
bt_setbaudrate_start =0;
//change MSP430 UART to use new baud rate
setupUART(userBaudRate);
if(!strncmp(userBaudRate, "1200", 4) || !strncmp(userBaudRate, "2400", 4)) {
slowRate = 1;
} else {
slowRate = 0;
}
}
}
return 0;
}
void main(void)
{
uint8_t i;
WDTCTL = WDTPW + WDTHOLD; // Stop watchdog timer
BCSCTL1 = CALBC1_16MHZ; // Run at 16 MHz
DCOCTL = CALDCO_16MHZ; // Run at 16 MHz
P1DIR |= BIT0;
SPI_init();
AD5504_init(&ad5504);
DAC7512_init(&dac7512);
TA1CCR0 = 100; // Set timer to 100 time length intervals.
//Set TimerA to use auxiliary clock in UP mode
TA1CTL = TASSEL_2 | ID_2 | MC_1; // Select the SM_CLK which is running at 16MHz/4
//Enable the interrupt for TACCR0 match
TA1CCTL0 = CCIE;
// Enable all ADC outputs and set to 0.
for (i = 0; i < 4; i++) {
AD5504_send(&ad5504, AD5504_CTRL, AD5504_CHA_ON | AD5504_CHB_ON | AD5504_CHC_ON | AD5504_CHD_ON, AD5504_WRITE, 1<<i);
}
for (i = 0; i < 16; i++) {
AD5504_send(&ad5504, (ad5504_addresses[i] & 0xff), ad5504_value_reg[i], AD5504_WRITE, ad5504_addresses[i]>>8);
DAC7512_send(&dac7512, dac7512_value_reg[i], (i>>2)&0x3, i&0x3);
}
uart_dev_init(&uart_dev);
setupUART(9600, UART_RXIE); // N = f_clk/Baudrate, Baudrate == 9600
__bis_SR_register(GIE); // interrupts enabled
while(1) {
}
}
void main(void)
{
configClocks();
setupOutputs();
//setupInterrupts();
//turnTXD(ON);
setupUART();
_EINT();
while(1){
//if (port) {
//turnTXD(ON);
//P1IFG &= ~ADR_BUS0;
//port = 0;
//blinkLEDGreen(1);
//blinkLEDRed(1);
//int i = 0, timeOut = 10000;
//while(i < timeOut){
while (byteRX == 'D') {
turnTXD(ON);
blinkLEDRed(1);
UART_putChar('O');
blinkLEDGreen(2);
turnTXD(OFF);
//byteRX = '#'; //this is to flush the RX buffer
//i = timeOut;
}
//blinkLEDRed(1);
//i++;
}
//}
}
void setup()
{
uint8_t i=0;
//初始化系统时钟
setupSystemClock();
//初始化串口
setupUART();
UART_NVIC_INIT();
//初始化System_tick
setup_system_tick(SYSTEM_TICK_FREQ);
//初始化IIC
I2C_Init();
//初始化FLASH
FlashInit();
LoadParamsFromFlash();
//初始化低电压检测
BatteryCheckInit();
//初始化遥控
Comm_Init();
//初始化LED
LED_Init();
//初始化SENSOR
#ifdef IMU_SW //软件姿态解算
// MPU6050_initialize();
// DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
#else
MPU6050_initialize();
DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
MPU6050_DMP_Initialize(); //初始化DMP引擎
#endif
//初始化自稳定
// LED_ON();
// //测试用，延迟启动时间
// for(i=0;i<6;i++)
// {
// DelayMsec(1000);
// LED_OFF();
// }
//初始化电机
Motor_Init();
//printf("Motor_Init(); \n");
//IMU_Init(); // sample rate and cutoff freq. sample rate is too low now due to using dmp.
// printf("\n\nCPU @ %dHz\n", SystemCoreClock);
//MotorPwmOutput(100,100,0,0);
}
/*********************************************************************
* @fn HidKbdMouse_Init
*
* @brief Initialization function for the HidKbdMouse App Task.
* This is called during initialization and should contain
* any application specific initialization (ie. hardware
* initialization/setup, table initialization, power up
* notificaiton ... ).
*
* @param task_id - the ID assigned by OSAL. This ID should be
* used to send messages and set timers.
*
* @return none
*/
void HidKbdMouse_Init( uint8 task_id )
{
setupUART();
GenerateCRCTable();
//printf("%x\n",default_name_crc);
device_name = osal_mem_alloc(20);
device_name_length = osal_mem_alloc(1);
device_name_crc = osal_mem_alloc(1);
hidKbdMouseTaskId = task_id;
// Setup the GAP
VOID GAP_SetParamValue( TGAP_CONN_PAUSE_PERIPHERAL, DEFAULT_CONN_PAUSE_PERIPHERAL );
// Setup the GAP Peripheral Role Profile
{
uint8 initial_advertising_enable = TRUE; //previously FALSE
// By setting this to zero, the device will go into the waiting state after
// being discoverable for 30.72 second, and will not being advertising again
// until the enabler is set back to TRUE
uint16 gapRole_AdvertOffTime = 0;
uint8 enable_update_request = DEFAULT_ENABLE_UPDATE_REQUEST;
uint16 desired_min_interval = DEFAULT_DESIRED_MIN_CONN_INTERVAL;
uint16 desired_max_interval = DEFAULT_DESIRED_MAX_CONN_INTERVAL;
uint16 desired_slave_latency = DEFAULT_DESIRED_SLAVE_LATENCY;
uint16 desired_conn_timeout = DEFAULT_DESIRED_CONN_TIMEOUT;
// Set the GAP Role Parameters
GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &initial_advertising_enable );
GAPRole_SetParameter( GAPROLE_ADVERT_OFF_TIME, sizeof( uint16 ), &gapRole_AdvertOffTime );
GAPRole_SetParameter( GAPROLE_ADVERT_DATA, sizeof( advData ), advData );
//name change - 21/12/2014
osal_snv_read(SNV_ID_DEVICE_NAME_CRC, 1, device_name_crc);
osal_snv_read(SNV_ID_DEVICE_NAME_LENGTH, 1, device_name_length);
osal_snv_read(SNV_ID_DEVICE_NAME, 20, device_name);
if(*device_name_crc != getCRC(device_name, *device_name_length)) {
printf("Using default scan response name\r\n");
GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, sizeof ( default_scanData ), default_scanData );
} else {
//make changes directly to the default_scanData. Since this variable is set at start-up, it should not matter
uint8 len = *device_name_length;
//uint8 default_name_length = default_scanData[0];
default_scanData[0] = len + 1;
uint8 i;
for(i = 0; i < len; i++) {
default_scanData[i+2] = device_name[i];
}
GAPRole_SetParameter( GAPROLE_SCAN_RSP_DATA, sizeof ( default_scanData ), default_scanData );
}
GAPRole_SetParameter( GAPROLE_PARAM_UPDATE_ENABLE, sizeof( uint8 ), &enable_update_request );
GAPRole_SetParameter( GAPROLE_MIN_CONN_INTERVAL, sizeof( uint16 ), &desired_min_interval );
GAPRole_SetParameter( GAPROLE_MAX_CONN_INTERVAL, sizeof( uint16 ), &desired_max_interval );
GAPRole_SetParameter( GAPROLE_SLAVE_LATENCY, sizeof( uint16 ), &desired_slave_latency );
GAPRole_SetParameter( GAPROLE_TIMEOUT_MULTIPLIER, sizeof( uint16 ), &desired_conn_timeout );
}
// Set the GAP Characteristics
if(*device_name_crc != getCRC(device_name, *device_name_length)) {
// printf("Using default device name");
GGS_SetParameter( GGS_DEVICE_NAME_ATT, GAP_DEVICE_NAME_LEN, (void *) attDeviceName );
} else {
// printf("Using stored device name");
// printf("%s\r\n", device_name);
GGS_SetParameter( GGS_DEVICE_NAME_ATT, *device_name_length + 1, (void *) device_name );
}
//Allow device to change name
uint8 devNamePermission = GATT_PERMIT_READ|GATT_PERMIT_WRITE;
GGS_SetParameter( GGS_W_PERMIT_DEVICE_NAME_ATT, sizeof ( uint8 ), &devNamePermission );
// Setup the GAP Bond Manager
{
uint32 passkey = DEFAULT_PASSCODE;
uint8 pairMode = DEFAULT_PAIRING_MODE;
uint8 mitm = DEFAULT_MITM_MODE;
uint8 ioCap = DEFAULT_IO_CAPABILITIES;
uint8 bonding = DEFAULT_BONDING_MODE;
GAPBondMgr_SetParameter( GAPBOND_DEFAULT_PASSCODE, sizeof( uint32 ), &passkey );
GAPBondMgr_SetParameter( GAPBOND_PAIRING_MODE, sizeof( uint8 ), &pairMode );
GAPBondMgr_SetParameter( GAPBOND_MITM_PROTECTION, sizeof( uint8 ), &mitm );
GAPBondMgr_SetParameter( GAPBOND_IO_CAPABILITIES, sizeof( uint8 ), &ioCap );
GAPBondMgr_SetParameter( GAPBOND_BONDING_ENABLED, sizeof( uint8 ), &bonding );
}
// Setup Battery Characteristic Values
{
uint8 critical = DEFAULT_BATT_CRITICAL_LEVEL;
Batt_SetParameter( BATT_PARAM_CRITICAL_LEVEL, sizeof (uint8), &critical );
}
// Set up HID keyboard service
HidKbM_AddService( );
// Register for HID Dev callback
HidDev_Register( &hidKbdMouseCfg, &hidKbdMouseHidCBs );
// Register for all key events - This app will handle all key events
RegisterForKeys( hidKbdMouseTaskId );
#if defined( CC2540_MINIDK )
// makes sure LEDs are off
HalLedSet( (HAL_LED_1 | HAL_LED_2), HAL_LED_MODE_OFF );
// For keyfob board set GPIO pins into a power-optimized state
// Note that there is still some leakage current from the buzzer,
// accelerometer, LEDs, and buttons on the PCB.
P0SEL = 0; // Configure Port 0 as GPIO
P1SEL = 0; // Configure Port 1 as GPIO
P2SEL = 0; // Configure Port 2 as GPIO
P0DIR = 0xFC; // Port 0 pins P0.0 and P0.1 as input (buttons),
// all others (P0.2-P0.7) as output
P1DIR = 0xFF; // All port 1 pins (P1.0-P1.7) as output
P2DIR = 0x1F; // All port 1 pins (P2.0-P2.4) as output
P0 = 0x03; // All pins on port 0 to low except for P0.0 and P0.1 (buttons)
P1 = 0; // All pins on port 1 to low
P2 = 0; // All pins on port 2 to low
#endif // #if defined( CC2540_MINIDK )
//init keyboard report manager
KBD_Report_Init();
// Setup a delayed profile startup
osal_set_event( hidKbdMouseTaskId, START_DEVICE_EVT );
}
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);
}
}
}
