void init_peripherals()
{
// LED
DDRC |= (1<<2);
// accel pins
lis3l_init();
// UART
uartInit();
uartSetBaudRate(19200);
uartSetFrameFormat(8, 0, 1);
rprintfInit(uartAddToTxBuffer);
cbi(DDRB, 1); // XBee CTS on PB1
// I2C
itg3200_i2cInit(200);
sbi(PORTC, 0); // i2c SCL on ATmega163,323,16,32,etc
sbi(PORTC, 1); // i2c SDA on ATmega163,323,16,32,etc
cbi(TWCR, TWIE); // disable interrupt
// itg3200_i2cSetBitrate(200); // todo, check if if w ecan do 200
// SPI
mySpiInit();
// a2d
a2dInit();
a2dSetReference(ADC_REFERENCE_256V);
a2dSetChannel(7);
cbi(PORTA, 7);
cbi(DDRA, 7);
a2dStartConvert();
_delay_ms(50);
// accel
BOOL accelOkay = lis3l_Reset();
itg3200_init();
// switch xbee to higher baudrate
// rprintfStr("+++");
// _delay_ms(55);
// rprintfStr("ATBD6,CN\r");
}
void appInitHardware(void){
// Initialise the servo controller
// Initialise the SPI bus
spiBusInit(&spiBus,true);
//servosInit(&bank1, TIMER1);
// Set UART0 to 9600 baud
uartInit(UART0, 9600);
// Tell rprintf to output to UART0
rprintfInit(&uart0SendByte);
}
int main(void)
{
uartInit();
uartSetBaudRate(115200);
rprintfInit(uartSendByte);
a2dInit();
timerInit();
vt100Init();
return 0;
}
void _servosCenter(SERVO_LIST* const servos, uint8_t numServos, UART* uart){
rprintfInit(uart->writer);
list(servos,numServos);
help();
prompt();
while(1){
char c = getCh(uart);
switch(c){
case 'L':
list(servos,numServos);
prompt();
break;
case '+':
update(servos,1);
break;
case '*':
update(servos,10);
break;
case '-':
update(servos,-1);
break;
case '/':
update(servos,-10);
break;
case 'N':
current = (current+1) % numServos;
prompt();
break;
case 'P':
current = (current==0) ? numServos-1 : current-1;
prompt();
break;
case 'C':
setRanging(FALSE, servos,numServos);
prompt();
break;
case 'R':
setRanging(TRUE, servos,numServos);
prompt();
break;
default:
help();
prompt();
break;
}
}
}
int main(void)
{
timerInit(); // initializing timers
uartInit(); // initializing serial port
uartSetBaudRate(115200); // set serial port baud rate
rprintfInit(uartSendByte); // direct rprintf() output to go to serial port
timerPause(100); // wait for a moment
// print welcome message
rprintf("\r\n\n\nWelcome to the Spyglass UI test.\r\n");
// begin test application
spyglassTest();
return 0;
}
//----- Begin Code ------------------------------------------------------------
int main(void)
{
// initialize our libraries
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(115200);
// initialize rprintf system
rprintfInit(uartSendByte);
// initialize timers
timerInit();
// run the test
extintTest();
return 0;
}
inline void init(){
// Global initializations
uartComms.init();
init_PWM();
init_ADC();
rprintfInit(uartSendByte);
// DEBUGprint init
// init_DEBUGprint();
// rprintf("*I\n");
// Wait for Eeprom to become available
eeprom_busy_wait();
startupDiagnostics();
}
void init_uart(void) {
// initialize our libraries
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(9600);
// set uartSendByte as the output for all rprintf statements
rprintfInit(uartSendByte);
// initialize the timer system
timerInit();
// initialize vt100 library
vt100Init();
vt100ClearScreen();
// print a little intro message so we know things are working
rprintf("\r\nServo tester\r\n");
rprintf("Sebastien CELLES\r\n");
rprintf("IUT de Poitiers\r\n");
rprintf("Genie Thermique et Energie\r\n");
}
int main(void)
{
// initialize our libraries
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(9600);
// set uartSendByte as the output for all rprintf statements
rprintfInit(uartSendByte);
// initialize the timer system
timerInit();
// print a little intro message so we know things are working
rprintf("\r\nWelcom to Servo Test!\r\n");
// being servo test
servoTest();
return 0;
}
//----- Begin Code ------------------------------------------------------------
int main(void)
{
// initialize our libraries
// initialize the UART (serial port)
uartInit();
uartSetBaudRate(115200);
// make all rprintf statements use uart for output
rprintfInit(uartSendByte);
// initialize the timer system
timerInit();
// clear terminal screen
vt100ClearScreen();
vt100SetCursorPos(1,1);
// print a little intro message so we know things are working
rprintf("\r\nWelcome to ADS7828 test!\r\n");
// run tests
ads7828test();
return 0;
}
//----- Begin Code ------------------------------------------------------------
int main(void)
{
// initialize our libraries
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(9600);
// initialize the timer system
timerInit();
// initialize rprintf system
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
// run the test
encoderTest();
return 0;
}
void init(void)
{
// Initialize Timer
timerInit();
// Initialize LCD
lcdInit();
ourLcdControlWrite(1<<LCD_ON_CTRL | 1<<LCD_ON_DISPLAY);
// Initialize UART
uartInit();
uartSetBaudRate(CMU_BAUD);
uartSetRxHandler(packetRcv);
rprintfInit(uartSendByte);
// Initialize PWM
outb(DDRD, 0xFF); // set all port D pins to output
timer1PWMInit(8);
timer1PWMAOn();
timer1PWMBOn();
// Initialize Servos
servoInit();
// Initialize CMU
lcdWriteStr("CMUcam2 init", 0, 0);
cmuInit();
}
int main(void)
{
// initialize processor
processorInit();
// initialize timers
timerInit();
// initialize uarts
uart0Init(UART_BAUD(115200), UART_8N1, UART_FIFO_8);
uart1Init(UART_BAUD(115200), UART_8N1, UART_FIFO_8);
// initialize rprintf to use UART1 for output
rprintfInit(uart1SendByte);
// Wait for a moment to allow hardware to stabilize.
// This may be important if a serial port level-converter
// like the MAX232 is used. Charge-pump based converters
// need some time after power-up to charge before
// communication is reliable.
timerPause(50); // waits 50 milliseconds
// run the test
uartTest();
return 0;
}
int main(void)
{
struct netEthAddr myEthAddress;
timerInit();
uartInit();
uartSetBaudRate(9600);
rprintfInit(uartSendByte);
timerPause(100);
rprintf("\r\nNetwork Stack Example\r\n");
// initialize systick timer
rprintf("Initializing Periodic Timer\r\n");
timer2SetPrescaler(TIMER_CLK_DIV1024);
timerAttach(TIMER2OVERFLOW_INT, systickHandler);
// init network stack
rprintf("Initializing Network Stack\r\n");
netstackInit(IPADDRESS, NETMASK, GATEWAY);
nicGetMacAddress(&myEthAddress.addr[0]);
rprintfProgStrM("Eth Addr is: "); netPrintEthAddr(&myEthAddress); rprintfCRLF();
rprintfProgStrM("IP Addr is: "); netPrintIPAddr(ipGetConfig()->ip); rprintfCRLF();
rprintf("Network Stack is up!\r\n");
rprintf("Starting packet receive loop\r\n");
while (1)
{
// service local stuff
serviceLocal();
// service the network
netstackService();
}
return 0;
}
void setup() {
uart0Init();
uartSetBaudRate(0, 9600);
rprintfInit(uart0SendByte);
rprintf("$Id$");
// this goes to the switch common
sbi(DDRA, DDA0); // output on PA0
sbi(PORTA, PA0); // take it high
cbi(DDRA, DDA1);
cbi(DDRA, DDA2);
cbi(DDRA, DDA3);
cbi(DDRA, DDA4);
cbi(DDRA, DDA5);
a2dInit();
a2dSetReference(ADC_REFERENCE_AVCC);
a2dSetPrescaler(ADC_PRESCALE_DIV128);
sbi(DDRB, PB3); // output for the LED
sbi(PORTB, PB3);
}
int main(void)
{
uint8_t system_state=0, i2c_resets=0, si446x_resets=0;//used to track button press functionality and any errors
uint8_t sensors=0;
uint32_t repetition_counter=0; //Used to detect any I2C lockup
uint8_t L3GD20_Data_Buffer_old[8]; //Used to test for noise in the gyro data (indicating that it is working)
uint8_t UplinkFlags=0,CutFlags=0;
uint16_t UplinkBytes=0; //Counters and flags for telemetry
uint32_t last_telemetry=0,cutofftime=0,indtest=0,badgyro=0,permission_time=0,countdown_time=0,last_cuttest=0;
uint16_t sentence_counter=0;
uint8_t silab;
//Cutdown config stuff here, atm uses hardcoded polygon defined in polygon.h
static const int32_t Geofence[UK_GEOFENCE_POINTS*2]=UK_GEOFENCE;
RTC_t RTC_time;
_REENT_INIT_PTR(&my_reent);
_impure_ptr = &my_reent;
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE);/* Enable PWR and BKP clocks */
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
uint16_t shutdown_lock=BKP_ReadBackupRegister(BKP_DR3); //Holds the shutdown lock setting
uint16_t reset_counter=BKP_ReadBackupRegister(BKP_DR2); //The number of consecutive failed reboot cycles
PWR_BackupAccessCmd(DISABLE);
if(RCC->CSR&RCC_CSR_IWDGRSTF && shutdown_lock!=SHUTDOWNLOCK_MAGIC) {//Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
if(USB_SOURCE==bootsource) {
RCC->CFGR &= ~(uint32_t)RCC_CFGR_PPRE1_DIV16;
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV4;//Swap the ABP1 bus to run at 12mhz rather than 4 if we booted from USB, makes USB fast enough
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if((Millis>10000 && bDeviceState == UNCONNECTED)|| !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_VBUS_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
PWR_BackupAccessCmd(ENABLE); /* Allow access to BKP Domain */
BKP_WriteBackupRegister(BKP_DR3,0x0000);//Wipe the shutdown lock setting
PWR_BackupAccessCmd(DISABLE);
while(1) {
if(!(Millis%1000) && bDeviceState == SUSPENDED) {
Delay(100);
if(!GET_VBUS_STATE)
shutdown();
}
Watchdog_Reset();
__WFI(); //Sleep mode
}
}
if(!GET_PWR_STATE && !(CoreDebug->DHCSR&0x00000001) && shutdown_lock!=SHUTDOWNLOCK_MAGIC) {//Check here to make sure the power button is still pressed, if not, sleep if no debug and not in always on flight mode
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
}
// check to see if battery has enough charge to start
ADC_Configuration(); //We leave this a bit later to allow stabilisation
{
uint32_t t=Millis;
while(Millis<(t+100)){__WFI();} //Sensor+inst amplifier takes about 100ms to stabilise after power on
}
if(Battery_Voltage<BATTERY_STARTUP_LIMIT) { //We will have to turn off
if(reset_counter<10)
shutdown();
}
Watchdog_Reset(); //Card Init can take a second or two
// system has passed battery level check and so file can be opened
{//Context
uint8_t silabs_header[5]={};
uint8_t* silabs_header_=NULL; //Pointer to the array (if all goes ok)
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else { //FATFS initialised ok, try init the card, this also sets up the SPI1
if(!(f_err_code=f_open(&FATFS_logfile,(const TCHAR*)"time.txt",FA_OPEN_EXISTING|FA_READ|FA_WRITE))){//Try to open time file get system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(FATFS_info.fsize<5) { //Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile); //Close the time.txt file
}
// load settings if file exists
Watchdog_Reset(); //Card Init can take a second or two
if(!f_open(&FATFS_logfile,(const TCHAR *)"settings.dat",FA_OPEN_EXISTING | FA_READ)) {
UINT br;
int8_t rtc_correction;
f_read(&FATFS_logfile, (void*)(&rtc_correction),sizeof(rtc_correction),&br);
//Use the setting to apply correction to the RTC
if(br && (rtc_correction<30) && (rtc_correction>-92) && rtc_correction ) {
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
uint16_t tweaked_prescale = (0x0001<<15)-2;/* Try to run the RTC slightly too fast so it can be corrected either way */
RTC_WaitForSynchro(); /* Wait for RTC registers synchronization */
if( RTC->PRLL != tweaked_prescale ) {/*Note that there is a 0.5ppm offset here (correction 0==0.5ppm slow)*/
RTC_SetPrescaler(tweaked_prescale); /* RTC period = RTCCLK/RTC_PR = (32.768 KHz)/(32767-2+1) */
RTC_WaitForLastTask();
}
BKP_SetRTCCalibrationValue((uint8_t) ((int16_t)31-(21*(int16_t)rtc_correction)/(int16_t)20) );
BKP_RTCOutputConfig(BKP_RTCOutputSource_None);/* Ensure any output is disabled here */
/* Lock access to BKP Domain */
PWR_BackupAccessCmd(DISABLE);
}
else if(br && ((uint8_t)rtc_correction==0x91) ) {/* 0x91 magic flag sets the RTC clock output on */
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
BKP_RTCOutputConfig(BKP_RTCOutputSource_CalibClock);/* Output a 512Hz reference clock on the TAMPER pin*/
PWR_BackupAccessCmd(DISABLE);
}
if(br) {
f_read(&FATFS_logfile, (void*)(&shutdown_lock),sizeof(shutdown_lock),&br);/*This needs to be set with the same magic flag value*/
if(br==2) {
PWR_BackupAccessCmd(ENABLE);/* Allow access to BKP Domain */
BKP_WriteBackupRegister(BKP_DR3,shutdown_lock);//Wipe the shutdown lock setting
PWR_BackupAccessCmd(DISABLE);
}
}
if(br==2) { //Read was successful, next try to read 5 bytes of packet header
f_read(&FATFS_logfile, (void*)(silabs_header),5,&br);
if(br!=5)
silabs_header_=silabs_header;
}
f_close(&FATFS_logfile); //Close the settings.dat file
}
#ifndef SINGLE_LOGFILE
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
printf("%02d-%02d-%02dT%02d-%02d-%02d-%s.csv",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec,"Log");//Timestamp name
rprintfInit(__usart_send_char); //Printf over the bluetooth
#endif
Watchdog_Reset(); //Card Init can take a second or two
if((f_err_code=f_open(&FATFS_logfile,(TCHAR*)LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
Delay(10000);
if((f_err_code=f_open(&FATFS_logfile,(TCHAR*)LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Try again
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
}
else {
Watchdog_Reset(); //Card Init can take a second or two
print_string[strlen(print_string)-4]=0x00;//Wipe the .csv off the string
strcat(print_string,"_gyro.wav");
if((f_err_code=f_open(&FATFS_wavfile_gyro,(TCHAR*)LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
else { //We have a mounted card
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=1; //So we know to close the file properly on shutdown
if(file_opened==1) {
Watchdog_Reset(); //Card Init can take a second or two
if (f_err_code || f_tell(&FATFS_wavfile_gyro) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_wavfile_gyro);//Close the already opened file on error
else
file_opened|=2; //So we know to close the file properly on shutdown
}
}
}
}
f_err_code|=write_wave_header(&FATFS_wavfile_gyro, 4, 100, 16);//4 channels, last channel is for the current rpm
Watchdog_Reset(); //Card Init can take a second or two
//Setup and test the silabs radio
silab=si446x_setup(silabs_header_);
if(silab!=0x44) { //Should return the device code
print_string[0]=0x00;
printf("Silabs: %02x\n",silab);
f_puts("Silabs detect error, got:",&FATFS_logfile);
f_puts(print_string,&FATFS_logfile);
shutdown_filesystem(ERR, file_opened);//So we log that something went wrong in the logfile
shutdown();
}
}//Context
int main(void)
{
uint32_t data_counter=0; //used as data timestamp
uint8_t deadly_flashes=0,system_state=0,repetition_counter=0;
int16_t sensor_data, sensor_raw_data[3]={}; //used for handling data passed back from sensors
int16_t sfe_sensor_ref_buff[2][3],sfe_sensor_ref_buff_old[2][3];//used to detect and fix I2C bus lockup
RTC_t RTC_time;
wave_stuffer Gyro_wav_stuffer={0,0},Accel_wav_stuffer={0,0};//Used to controlling wav file bit packing
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
if(RCC->CSR&RCC_CSR_IWDGRSTF) { //Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if(Millis>10000 || !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_CHRG_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
USB_Configured_LED();
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
while(1) { //If running off USB (mounted as mass storage), stay in this loop - dont turn on anything
if(Millis%1000>500) //1Hz on/off flashing
switch_leds_on(); //Flash the LED(s)
else
switch_leds_off();
Watchdog_Reset();
__WFI(); //Sleep until something arrives
}
}
else {
if(!GET_PWR_STATE) //Check here to make sure the power button is still pressed, if not, sleep
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
ADC_Configuration(); //At present this is purely here to detect low battery
do {
battery_voltage=Battery_Voltage;//Have to flush adc for some reason
Delay(25000);
} while(fabs(Battery_Voltage-battery_voltage)>0.01 || !battery_voltage);
I2C_Config(); //Setup the I2C bus
Sensors=detect_sensors(0); //Search for connected sensors
if(battery_voltage<BATTERY_STARTUP_LIMIT)
deadly_flashes=1;
if(!(Sensors&(1<<FOREHEAD_ACCEL))) //Check for any missing sensors
deadly_flashes=2;
if(!(Sensors&(1<<(FOREHEAD_GYRO-1))))
deadly_flashes=3;
if(!(Sensors&(1<<(SFE_1_ACCEL-1))))
deadly_flashes=4;
if(!(Sensors&(1<<(SFE_1_MAGNO-1))))
deadly_flashes=5;
if(!(Sensors&(1<<(SFE_1_GYRO-1))))
deadly_flashes=6;
if(!(Sensors&(1<<(SFE_2_ACCEL-3))))
deadly_flashes=7;
if(!(Sensors&(1<<(SFE_2_MAGNO-3))))
deadly_flashes=8;
if(!(Sensors&(1<<(SFE_2_GYRO-3))))
deadly_flashes=9;
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else if(!deadly_flashes){ //FATFS and the I2C initialised ok, try init the card, this also sets up the SPI1
if(!f_open(&FATFS_logfile,"time.txt",FA_OPEN_EXISTING | FA_READ | FA_WRITE)) {//Try and open a time file to get the system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(!FATFS_info.fsize) {//Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile);//Close the time.txt file
}
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
//timestamp name
printf("%d-%02d-%02dT%02d-%02d-%02d",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec);
rprintfInit(__usart_send_char); //Printf over the bluetooth
f_err_code = f_mkdir(print_string); //Try to make a directory where the logfiles will live
if(f_err_code) {
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
repetition_counter=1;
}
else
f_err_code=f_chdir(print_string);//enter our new directory
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error entering direcotry %d\r\n",f_err_code);
repetition_counter=1;
}
else {
strcat(print_string,".csv");
f_err_code=f_open(&FATFS_logfile,print_string,FA_CREATE_ALWAYS | FA_WRITE);//Try to open the main 100sps csv logfile
}
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error creating logfile %d\r\n",f_err_code);
repetition_counter=1;
}
else {
print_string[strlen(print_string)-4]=0x00; //Wipe the .csv off the string
strcat(print_string,"_accel.wav");
f_err_code=f_open(&FATFS_wavfile_accel,print_string,FA_CREATE_ALWAYS | FA_WRITE);//Try to open the accel wav logfile
}
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error creating accel wav file %d\r\n",f_err_code);
repetition_counter=1;
}
else {
print_string[strlen(print_string)-9]=0x00; //Wipe the accel.wav off the string
strcat(print_string,"gyro.wav");
f_err_code=f_open(&FATFS_wavfile_gyro,print_string,FA_CREATE_ALWAYS | FA_WRITE);//Try to open the gyro wav logfile
}
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error creating gyro wav file %d\r\n",f_err_code);
}
else { //We have a mounted card
print_string[0]=0x00; //Wipe the string
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=0x01;//So we know to close the file properly on shutdown - bit mask for the files
if(!f_err_code) {
f_err_code=f_lseek(&FATFS_wavfile_accel, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_wavfile_accel) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_wavfile_accel, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
}
if(f_err_code)
f_close(&FATFS_wavfile_accel);//Close the already opened file on error
else
file_opened|=0x02;//So we know to close the file properly on shutdown - bit mask for the files
}
if(!f_err_code) {
f_err_code=f_lseek(&FATFS_wavfile_gyro, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_wavfile_gyro) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_wavfile_gyro, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
}
if(f_err_code)
f_close(&FATFS_wavfile_gyro);//Close the already opened file on error
else
file_opened|=0x04;//So we know to close the file properly on shutdown - bit mask for the files
}
}
}
repetition_counter=0; //Reset this here
//We die, but flash out a number of flashes first
if(f_err_code || deadly_flashes) { //There was an init error
for(;deadly_flashes;deadly_flashes--) {
RED_LED_ON;
Delay(200000);
RED_LED_OFF;
Delay(200000);
Watchdog_Reset();
}
RED_LED_ON;
Delay(400000);
shutdown(); //Abort after a (further )single red flash
}
}
int main(void)
{
network_init();
//CLKPR = (1<<CLKPCE); //Change prescaler
//CLKPR = (1<<CLKPS0); //Use prescaler 2
//clock_prescale_set(clock_div_2);
enc28j60Write(ECOCON, 1 & 0x7); //Get a 25MHz signal from enc28j60
uartInit();
uartSetBaudRate(9600);
rprintfInit(uartSendByte);
int i;
uip_ipaddr_t ipaddr;
struct timer periodic_timer, arp_timer;
clock_init();
timer_set(&periodic_timer, CLOCK_SECOND / 2);
timer_set(&arp_timer, CLOCK_SECOND * 10);
uip_init();
struct uip_eth_addr mac = {UIP_ETHADDR0, UIP_ETHADDR1, UIP_ETHADDR2, UIP_ETHADDR3, UIP_ETHADDR4, UIP_ETHADDR5};
uip_setethaddr(mac);
// set up the udp data stream
// configure the target system ip and port
uip_ipaddr_t target_ipaddr;
uip_ipaddr(&target_ipaddr, 192,168,0,10);
udpds_conf(&target_ipaddr, 1000, 1);
#ifdef __DHCPC_H__
dhcpc_init(&mac, 6);
#else
uip_ipaddr(ipaddr, 192,168,0,1);
uip_sethostaddr(ipaddr);
uip_ipaddr(ipaddr, 192,168,0,1);
uip_setdraddr(ipaddr);
uip_ipaddr(ipaddr, 255,255,255,0);
uip_setnetmask(ipaddr);
#endif /*__DHCPC_H__*/
while(1){
uip_len = network_read();
if(uip_len > 0) {
if(BUF->type == htons(UIP_ETHTYPE_IP)){
uip_arp_ipin();
uip_input();
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}else if(BUF->type == htons(UIP_ETHTYPE_ARP)){
uip_arp_arpin();
if(uip_len > 0){
network_send();
}
}
}else if(timer_expired(&periodic_timer)) {
timer_reset(&periodic_timer);
for(i = 0; i < UIP_CONNS; i++) {
uip_periodic(i);
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}
#if UIP_UDP
for(i = 0; i < UIP_UDP_CONNS; i++) {
uip_udp_periodic(i);
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}
#endif /* UIP_UDP */
if(timer_expired(&arp_timer)) {
timer_reset(&arp_timer);
uip_arp_timer();
}
}
}
return 0;
}
//----- Begin Code ------------------------------------------------------------
int main(void)
{
// init a few string variables:
char *welcome_msg1 = " LC Meter III ";
char *welcome_msg2 = "dansworkshop.com";
char *cal_message1 = "Switch to Cal/C ";
char *cal_message2 = "for calibration.";
char *cal_message3 = "calibrating... ";
char *blank_lcd_line = " ";
// initialize LCD
lcdInit();
// init timers and I/O:
init();
sei(); // enable global interrupts
// direct printf output to LCD
rprintfInit(lcdDataWrite);
// print vanity message on LCD for a second:
rprintfStr(welcome_msg1);
lcdGotoXY(0,1);
rprintfStr(welcome_msg2);
_delay_ms(1000);
// initialize the UART (serial port)
uartInit();
// make all rprintf statements use uart for output
rprintfInit(uartSendByte);
// print a little intro message so we know things are working
rprintfStr(welcome_msg1);
rprintf("\r\nhttp://www.");
rprintfStr(welcome_msg2);
rprintf("\r\n");
// instruct about setting mode switch to C for calibration
if(bit_is_clear(PIND, 4)) {
rprintfInit(lcdDataWrite);
lcdGotoXY(0,0);
rprintfStr(cal_message1);
lcdGotoXY(0,1);
rprintfStr(cal_message2);
rprintfInit(uartSendByte);
rprintfStr(cal_message1);
rprintfStr(cal_message2);
rprintf("\r\n");
}
while(bit_is_clear(PIND, 4)) {
// wait for user to switch mode to C
}
// send out the calibration message:
rprintfStr(cal_message3);
rprintf("\r\n");
rprintfInit(lcdDataWrite);
lcdGotoXY(0,0);
rprintfStr(cal_message3);
lcdGotoXY(0,1);
rprintfStr(blank_lcd_line);
rprintfInit(uartSendByte);
_delay_ms(1600);
F1 = running; // get open frequency
rprintfNum(10, 10, 0, ' ', F1 * 5);
rprintf("\r\n");
sbi(PORTD, PD3); // energize relay
_delay_ms(1000); // stabilize
F2 = running; // get test frequency
rprintfNum(10, 10, 0, ' ', F2 * 5);
rprintf("\r\n");
cbi(PORTD, PD3); // turn off relay
// do some floating point:
Cs = square(F2 * 5) * (.00000000092 / (square(F1 * 5) - square(F2 * 5))); // this should fit in a 64-bit value
Ls = 1 / (4 * square(M_PI) * square(F1 * 5) * Cs);
// everything out of the lcd for now:
rprintfInit(lcdDataWrite);
// enable PC5 as output
sbi(DDRC, PC5);
while (1) {
_delay_ms(200);
lcdGotoXY(0,0);
if(bit_is_clear(PIND, 4)) { // inductance measurement mode
if(running < 3) {
rprintf("Not an inductor \r");
} else {
Lt = (square(F1 * 5) / (square(running * 5)) - 1) * Ls;
rprintf("Lx: ");
if(Lt > .0001) {
rprintfFloat(4, Lt * 1000);
rprintf("mH");
}
else if(Lt > .0000001) {
rprintfFloat(4, Lt * 1000000);
rprintf("uH");
}
else {
rprintfFloat(4, Lt * 1000000000);
rprintf("nH");
}
rprintf(" \r");
}
} else { // capacitance measurement mode
if(running < 300) {
rprintf("Not a capacitor \r");
} else {
Ct = (square(F1 * 5) / (square(running * 5)) - 1) * Cs;
rprintf("Cx: ");
if(Ct > .0001) {
rprintfFloat(4, Ct * 1000);
rprintf("mF");
}
else if(Ct > .0000001) {
rprintfFloat(4, Ct * 1000000);
rprintf("uF");
}
else if(Ct > .0000000001){
rprintfFloat(4, Ct * 1000000000);
rprintf("nF");
}
else {
rprintfFloat(4, Ct * 1000000000000);
rprintf("pF");
}
rprintf(" \r");
}
}
if(bit_is_clear(PIND, 7)) {
lcdGotoXY(0,0);
rprintf("zeroed \r");
lcdGotoXY(0,1);
rprintfStr(blank_lcd_line);
lcdGotoXY(0,0);
F1 = running;
}
while(bit_is_clear(PIND, 7)) {
// do nothing till the user lets go of the zero button
}
// display the
lcdGotoXY(0,1);
rprintfNum(10, 6, 0, ' ', running * 5);
rprintf("Hz ");
}
return 0;
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
// -------- Start Switch/Button-------
// Switch/Button - see switch.h
// To test if it is pressed then
if(SWITCH_pressed(&button)){
// pressed
}
// To test if it is released then
if(SWITCH_released(&button)){
// released
}
// -------- End Switch/Button-------
// -------- Start Marquee-------
// Marquee - see 'segled.h'
// Before using the Marquee you need to redirect rprintf to write to it
// This can be done using
Writer old = rprintfInit(marqueeGetWriter(&marquee));
// All rprintf output will then be sent to the marquee but will not
// display until an end-of-line eg "\n" has been sent. Example:-
// rprintf("Hello World\n");
// If the endDelay is non-zero then the marquee will scroll
// forever or until you call: marqueeStop(&marquee);
// If the endDelay is zero then the marquee will stop once
// the entire line has been shown ('one-shot' mode)
// In 'one-shot' mode then you may want to make sure that
// a previous line has finished before you display a second line.
// This can be done as follows:-
marqueeSetEndDelay(&marquee,0); // Make sure we are in one-shot mode
if(marqueeIsActive(&marquee)==FALSE){
if(loopCount==1){
rprintf("ABCDEFGHIJKLMNOPQRSTUVWXYZ\n");
}else{
rprintf("Loop=%u\n",(unsigned)loopCount); // Put the loop count
}
}
// Restore rprintf back to its previous location
rprintfInit(old);
// -------- End Marquee-------
// -------- Start Sharp GP2-------
// Read the Sharp GP2 and store the result in ir_0.distance.cm
distanceRead(ir_0);
// The value can be printed using %u eg rprintf("Distance=%u",ir_0.distance.cm);
// or dumped using:
rprintf("ir_0: ");
distanceDump(ir_0);
rprintfCRLF();
// -------- End Sharp GP2-------
// -------- Start Sharp GP2-------
// Read the Sharp GP2 and store the result in ir_1.distance.cm
distanceRead(ir_1);
// The value can be printed using %u eg rprintf("Distance=%u",ir_1.distance.cm);
// or dumped using:
rprintf("ir_1: ");
distanceDump(ir_1);
rprintfCRLF();
// -------- End Sharp GP2-------
// -------- Start Sharp GP2-------
// Read the Sharp GP2 and store the result in ir_2.distance.cm
distanceRead(ir_2);
// The value can be printed using %u eg rprintf("Distance=%u",ir_2.distance.cm);
// or dumped using:
rprintf("ir_2: ");
distanceDump(ir_2);
rprintfCRLF();
// -------- End Sharp GP2-------
// -------- Start Sharp GP2-------
// Read the Sharp GP2 and store the result in ir_3.distance.cm
distanceRead(ir_3);
// The value can be printed using %u eg rprintf("Distance=%u",ir_3.distance.cm);
// or dumped using:
rprintf("ir_3: ");
distanceDump(ir_3);
rprintfCRLF();
// -------- End Sharp GP2-------
// -------- Start Generic I2C Device-------
// Read 3 register values starting at register 1 into response
uint8_t lbRTOS_response[3];
if(i2cMasterReadRegisters(&lbRTOS.i2cInfo, 1, sizeof(lbRTOS_response), lbRTOS_response)){
// We have successfully read the data into 'lbRTOS_response'
}else{
rprintf("Failed reading lbRTOS\n");
}
// -------- End Generic I2C Device-------
return 0;
}
int main(void)
{
network_init();
#if MY_DEBUG
uartInit();
uartSetBaudRate(9600);
rprintfInit(uartSendByte);
#endif
int i;
uip_ipaddr_t ipaddr;
struct timer periodic_timer, arp_timer;
clock_init();
timer_set(&periodic_timer, CLOCK_SECOND / 2);
timer_set(&arp_timer, CLOCK_SECOND * 10);
uip_init();
struct uip_eth_addr mac = {UIP_ETHADDR0, UIP_ETHADDR1, UIP_ETHADDR2, UIP_ETHADDR3, UIP_ETHADDR4, UIP_ETHADDR5};
uip_setethaddr(mac);
telnetd_init();
#ifdef __DHCPC_H__
dhcpc_init(&mac, 6);
#else
uip_ipaddr(ipaddr, 192,168,0,1);
uip_sethostaddr(ipaddr);
uip_ipaddr(ipaddr, 192,168,0,1);
uip_setdraddr(ipaddr);
uip_ipaddr(ipaddr, 255,255,255,0);
uip_setnetmask(ipaddr);
#endif /*__DHCPC_H__*/
while(1){
uip_len = network_read();
if(uip_len > 0) {
if(BUF->type == htons(UIP_ETHTYPE_IP)){
uip_arp_ipin();
uip_input();
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}else if(BUF->type == htons(UIP_ETHTYPE_ARP)){
uip_arp_arpin();
if(uip_len > 0){
network_send();
}
}
}else if(timer_expired(&periodic_timer)) {
timer_reset(&periodic_timer);
for(i = 0; i < UIP_CONNS; i++) {
uip_periodic(i);
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}
#if UIP_UDP
for(i = 0; i < UIP_UDP_CONNS; i++) {
uip_udp_periodic(i);
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}
#endif /* UIP_UDP */
if(timer_expired(&arp_timer)) {
timer_reset(&arp_timer);
uip_arp_timer();
}
}
}
return 0;
}
void systickHandler(void)
{
uint16_t tms;
uint8_t ts,tm,th;
uint8_t pb;
// set timer for 10ms interval
TCNT2 = (unsigned char)-TIMER_INTERVAL;
// count up on uptime
UptimeMs += 10;
// if we at a 100ths millisecond interval, update the display
if(!(UptimeMs % 100))
{
// redirect rprintf() output to spyglass LCD
rprintfInit(spyglassLcdWriteChar);
// print banner message
spyglassLcdGotoXY(0,0);
rprintfProgStrM("**** SpyglassUI ****");
// read pushbuttons and take appropriate action
pb = spyglassGetPushbuttons();
spyglassLcdGotoXY(0,1);
rprintf("Buttons: ");
rprintfNum(2,8,FALSE,'0',pb);
if((pb & 0x01) && (Contrast < 255))
Contrast++;
if((pb & 0x02) && (Contrast > 0))
Contrast--;
if(pb & 0x08)
spyglassSetLeds(0x01);
if(pb & 0x10)
spyglassSetLeds(0x02);
// show display contrast
spyglassLcdGotoXY(0,2);
rprintf("LCD Contrast: ");
rprintfNum(10,3,FALSE,' ',Contrast);
spyglassSetLcdContrast(Contrast);
// show time
tms = (UptimeMs)%1000;
ts = (UptimeMs/1000)%60;
tm = (UptimeMs/60000l)%60;
th = (UptimeMs/3600000l);
spyglassLcdGotoXY(0,3);
rprintf("Time:");
rprintfNum(10,3,FALSE,' ',th);
rprintfChar(':');
rprintfNum(10,2,FALSE,'0',tm);
rprintfChar(':');
rprintfNum(10,2,FALSE,'0',ts);
rprintfChar('.');
rprintfNum(10,3,FALSE,'0',tms);
rprintf("ms");
// return rprintf() output to serial port
rprintfInit(uartSendByte);
}
}
int main(void)
{
/****************INITIALIZATIONS*******************/
//other stuff Im experimenting with for SoR
uartInit(); // initialize the UART (serial port)
uartSetBaudRate(0, 9600); // set UARTE speed, for Bluetooth
uartSetBaudRate(1, 115200); // set UARTD speed, for USB connection, up to 500k, try 115200 if it
uartSetBaudRate(2, 57600); // set UARTH speed
uartSetBaudRate(3, 57600); // set UARTJ speed, for Blackfin
//G=Ground, T=Tx (connect to external Rx), R=Rx (connect to external Tx)
// initialize rprintf system and configure uart1 (USB) for rprintf
rprintfInit(uart1SendByte);
configure_ports(); // configure which ports are analog, digital, etc.
LED_on();
rprintf("\r\nSystem Warming Up");
// initialize the timer system (comment out ones you don't want)
init_timer0(TIMER_CLK_1024);
init_timer1(TIMER_CLK_64);
init_timer2(TIMER2_CLK_64);
init_timer3(TIMER_CLK_64);
init_timer4(TIMER_CLK_64);
init_timer5(TIMER_CLK_1024);
//timer5Init();
a2dInit(); // initialize analog to digital converter (ADC)
a2dSetPrescaler(ADC_PRESCALE_DIV32); // configure ADC scaling
a2dSetReference(ADC_REFERENCE_AVCC); // configure ADC reference voltage
int i = 0, j = 0;
//let system stabelize for X time
for(i=0;i<16;i++)
{
j=a2dConvert8bit(i);//read each ADC once to get it working accurately
delay_cycles(5000); //keep LED on long enough to see Axon reseting
rprintf(".");
}
LED_off();
rprintf("Initialization Complete \r\n");
reset_timer0();
reset_timer1();
reset_timer2();
reset_timer3();
reset_timer4();
reset_timer5();
while(1)
{
control();
delay_cycles(100);
//an optional small delay to prevent crazy oscillations
}
return 0;
}
/*
vb
Returns multiple lines one for each blob
ix = vblob((unsigned char *)FRAME_BUF, (unsigned char*)FRAME_BUF3, ch1);
printf("##vb%c %d\r\n", ch2, ix);
for (iy=0; iy<ix; iy++) {
printf(" %d - %d %d %d %d \r\n",
blobcnt[iy], blobx1[iy], blobx2[iy], bloby1[iy],bloby2[iy]);
}
*/
uint8_t blackfinDetectBlobs (BLACKFIN_CAMERA* camera, uint8_t bin){
int32_t values[5];
BLACKFIN_BLOB* dest;
uint8_t actual = 0;
// allocate space for blob storage
if(camera->blob==null){
camera->blob = malloc(MAX_BLOBS * sizeof(BLACKFIN_BLOB)) ;
}
dest = camera->blob;
// Make rprintf go to _blackfin_command
Writer old = rprintfInit(&_blackfin_putcmd);
_blackfin_index=0;
rprintf("vb%d",bin);//start command, send bin #
// process the command
int args = __blackfinCommand(camera,PSTR("##vb"),PSTR(" vblob #"),values,2);
#ifdef BLACKFIN_DEBUG
_blackfin_set_active(camera->debug); //Send rprintf to the debugger
#endif
if(args==2 && values[0]==bin){
int8_t numBlobs = values[1]; // The number of blobs
if(numBlobs > MAX_BLOBS){
numBlobs = MAX_BLOBS;
}
#ifdef BLACKFIN_DEBUG
rprintf(" %d blobs\n",(int)numBlobs);
#endif
// Get each blob
for(int8_t num = 0; num < numBlobs; num++){
args = __blackfin_get_args(camera, values, 5, TRUE);
#ifdef BLACKFIN_DEBUG
rprintf(" #%d=",(int)num);
#endif
if(args==5){
dest->pixels = values[0];
dest->left = values[1];
dest->right = values[2];
dest->top = values[3];
dest->bottom = values[4];
dest->xCenter = dest->left + ((dest->right - dest->left)>>1);
dest->yCenter = dest->top + ((dest->bottom - dest->top)>>1);
#ifdef BLACKFIN_DEBUG
rprintf("Left,Top=");
rprintfNum(10,4,FALSE,'0',dest->left);
rprintfChar(',');
rprintfNum(10,4,FALSE,'0',dest->top);
rprintf(" Right,Bottom=");
rprintfNum(10,4,FALSE,'0',dest->right);
rprintfChar(',');
rprintfNum(10,4,FALSE,'0',dest->bottom);
rprintf(" Pixels=");rprintfNum(10,10,FALSE,' ',dest->pixels);
rprintfCRLF();
#endif
dest++;
actual++;
}else{
int main(void)
{
uint32_t ppg; //PPG channel
uint32_t data_counter=0; //used as data timestamp
uint8_t system_state=0; //used to track button press functionality
float sensor_data; //used for handling data passed back from sensors
RTC_t RTC_time;
_REENT_INIT_PTR(&my_reent);
_impure_ptr = &my_reent;
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
if(RCC->CSR&RCC_CSR_IWDGRSTF) { //Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if(Millis>10000 || !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_VBUS_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
USB_Configured_LED();
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
while(1) { //If running off USB (mounted as mass storage), stay in this loop - dont turn on anything
if(Millis%1000>500) //1Hz on/off flashing
switch_leds_on(); //Flash the LED(s)
else
switch_leds_off();
Watchdog_Reset();
}
}
if(!GET_PWR_STATE) //Check here to make sure the power button is still pressed, if not, sleep
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
for(uint8_t n=0;n<PPG_CHANNELS;n++)
init_buffer(&(Buff[n]),PPG_BUFFER_SIZE);//Enough for ~0.25S of data
setup_pwm(); //Enable the PWM outputs on all three channels
Delay(100000); //Sensor+inst amplifier takes about 100ms to stabilise after power on
ADC_Configuration(); //We leave this a bit later to allow stabilisation
calibrate_sensor(); //Calibrate the offset on the diff pressure sensor
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
I2C_Config(); //Setup the I2C bus
uint8_t sensors_=detect_sensors(); //Search for connected sensors
sensor_data=GET_BATTERY_VOLTAGE; //Have to flush adc for some reason
Delay(10000);
if(!(sensors_&~(1<<PRESSURE_HOSE))||GET_BATTERY_VOLTAGE<BATTERY_STARTUP_LIMIT) {//We will have to turn off
Watchdog_Reset(); //LED flashing takes a while
if(abs(Reported_Pressure)>PRESSURE_MARGIN)
Set_Motor(-1); //If the is air backpressure, dump to rapidly drop to zero pressure before turnoff
if(file_opened)
f_close(&FATFS_logfile); //be sure to terminate file neatly
red_flash();
Delay(400000);
red_flash(); //Two flashes means battery abort -----------------ABORT 2
if(sensors_&~(1<<PRESSURE_HOSE))
shutdown();
Delay(400000);
red_flash(); //Three flashes means no sensors abort ------------ABORT 3
shutdown();
}
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else { //FATFS initialised ok, try init the card, this also sets up the SPI1
if(!f_open(&FATFS_logfile,"time.txt",FA_OPEN_EXISTING | FA_READ | FA_WRITE)) {//Try and open a time file to get the system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(!FATFS_info.fsize) { //Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile); //Close the time.txt file
}
#ifndef SINGLE_LOGFILE
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
printf("%d-%d-%dT%d-%d-%d.txt",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec);//Timestamp name
rprintfInit(__usart_send_char); //Printf over the bluetooth
#endif
if((f_err_code=f_open(&FATFS_logfile,LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
else { //We have a mounted card
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=1; //So we know to close the file properly on shutdown
}
}
/*****************************************************************************
* Balance -
*****************************************************************************/
void balance(void)
{
unsigned long TimerMsWork;
long int g_bias = 0;
double x_offset = 532; //offset value 2.56V * 1024 / 4.93V = 4254
double q_m = 0.0;
double int_angle = 0.0;
double x = 0.0;
double tilt = 0.0;
int pwm;
InitADC();
init_pwm();
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(115200);
// initialize rprintf system
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
TimerMsWork = TimerMsCur();
DDRB |= (1 << PB0); // Make B0 an output for LED
/* as a 1st step, a reference measurement of the angular rate sensor is
* done. This value is used as offset compensation */
for (int i=1 ; i<=200; i++) // determine initial value for bias of gyro
{
g_bias = g_bias + GetADC(gyro_sensor);
}
g_bias = g_bias / 200;
while (!(getkey() == 1))
{
/* insure loop runs at specified Hz */
while (!TimerCheck(TimerMsWork, (dt_PARAM * 1000) -1))
;
TimerMsWork = TimerMsCur();
// toggle pin B0 for oscilloscope timings.
PORTB = PINB ^ (1 << PB0);
// get rate gyro reading and convert to deg/sec
// q_m = (GetADC(gyro_sensor) - g_bias) / -3.072; // -3.07bits/deg/sec (neg. because forward is CCW)
q_m = (GetADC(gyro_sensor) - g_bias) * -0.3255; // each bit = 0.3255 /deg/sec (neg. because forward is CCW)
state_update(q_m);
// get Accelerometer reading and convert to units of gravity.
// x = (GetADC(accel_sensor) - x_offset) / 204.9; // (205 bits/G)
x = (GetADC(accel_sensor) - x_offset) * 0.00488; // each bit = 0.00488/G
// x is measured in multiples of earth gravitation g
// therefore x = sin (tilt) or tilt = arcsin(x)
// for small angles in rad (not deg): arcsin(x)=x
// Calculation of deg from rad: 1 deg = 180/pi = 57.29577951
tilt = 57.29577951 * (x);
kalman_update(tilt);
int_angle += angle * dt_PARAM;
rprintf(" x:");
rprintfFloat(8, x);
rprintf(" angle:");
rprintfFloat(8, angle);
rprintf(" rate:");
rprintfFloat(8, rate);
// Balance. The most important line in the entire program.
// balance_torque = Kp * (current_angle - neutral) + Kd * current_rate;
// rprintf("bal_torq: ");
// rprintfFloat(8, balance_torque);
// rprintfCRLF();
//steer_knob = 0;
// change from current angle to something proportional to speed
// should this be the abs val of the cur speed or just curr speed?
//double steer_cmd = (1.0 / (1.0 + Ksteer2 * fabs(current_angle))) * (Ksteer * steer_knob);
//double steer_cmd = 0.0;
// Get current rate of turn
//double current_turn = left_speed - right_speed; //<-- is this correct
//double turn_accel = current_turn - prev_turn;
//prev_turn = current_turn;
// Closed-loop turn rate PID
//double steer_cmd = KpTurn * (current_turn - steer_desired)
// + KdTurn * turn_accel;
// //+ KiTurn * turn_integrated;
// Possibly optional
//turn_integrated += current_turn - steer_cmd;
// Differential steering
//left_motor_torque = balance_torque + steer_cmd; //+ cur_speed + steer_cmd;
//right_motor_torque = balance_torque - steer_cmd; //+ cur_speed - steer_cmd;
// Limit extents of torque demand
//left_motor_torque = flim(left_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (left_motor_torque < -MAX_TORQUE) left_motor_torque = -MAX_TORQUE;
// if (left_motor_torque > MAX_TORQUE) left_motor_torque = MAX_TORQUE;
//right_motor_torque = flim(right_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (right_motor_torque < -MAX_TORQUE) right_motor_torque = -MAX_TORQUE;
// if (right_motor_torque > MAX_TORQUE) right_motor_torque = MAX_TORQUE;
pwm = (int) ((angle -3.5) * Kp) + (rate * Kd); // + (int_angle * Ki);
rprintf(" pwm:%d\r\n", pwm);
// Set PWM values for both motors
SetLeftMotorPWM(pwm);
SetRightMotorPWM(pwm);
}
SetLeftMotorPWM(0);
SetRightMotorPWM(0);
}
int main(void)
{
network_init();
// network.c
// enc28j60Init();
// enc28j60PhyWrite(PHLCON,0x476);
//CLKPR = (1<<CLKPCE); //Change prescaler
//CLKPR = (1<<CLKPS0); //Use prescaler 2
//clock_prescale_set(clock_div_2);
enc28j60Write(ECOCON, 1 & 0x7);
// enc28j60.c
//Get a 25MHz signal from enc28j60
#ifdef MY_DEBUG
uartInit();
uartSetBaudRate(9600);
rprintfInit(uartSendByte);
#endif
int i;
uip_ipaddr_t ipaddr; // uip.h
// typedef u16_t uip_ip4addr_t[2];
// typedef uip_ip4addr_t uip_ipaddr_t;
struct timer periodic_timer, arp_timer;
clock_init();
timer_set(&periodic_timer, CLOCK_SECOND / 2);
timer_set(&arp_timer, CLOCK_SECOND * 10);
uip_init();
// uip.c
//uip_arp_init();
// uip_arp.c
// must be done or sometimes arp doesn't work
struct uip_eth_addr mac = {UIP_ETHADDR0, UIP_ETHADDR1, UIP_ETHADDR2, UIP_ETHADDR3, UIP_ETHADDR4, UIP_ETHADDR5};
uip_setethaddr(mac);
simple_httpd_init();
#ifdef __DHCPC_H__
dhcpc_init(&mac, 6);
#else
uip_ipaddr(ipaddr, 192,168,0,1); // uip.h
uip_sethostaddr(ipaddr); // uip.h
// #define uip_sethostaddr(addr) uip_ipaddr_copy(uip_hostaddr, (addr))
uip_ipaddr(ipaddr, 192,168,0,1);
uip_setdraddr(ipaddr);
// #define uip_setdraddr(addr) uip_ipaddr_copy(uip_draddr, (addr))
uip_ipaddr(ipaddr, 255,255,255,0);
uip_setnetmask(ipaddr);
// #define uip_setnetmask(addr) uip_ipaddr_copy(uip_netmask, (addr))
#endif /*__DHCPC_H__*/
while(1){
uip_len = network_read();
// uip.c : u16_t uip_len;
// network.c : return ((unt16_t) enc28j60PacketReceive(UIP_BUFSIZE, (uint8_t *)uip_buf));
// enc28j60.c : enc28j60PacketReceive
// uip.c : uint8_t uip_buf[UIP_BUFSIZE+2];
// uipconf.h : UIP_BUFSIZE:300
if(uip_len > 0) {
if(BUF->type == htons(UIP_ETHTYPE_IP)){
#ifdef MY_DEBUG
//rprintf("eth in : uip_len = %d, proto = %d\n", uip_len, uip_buf[23]);
//debugPrintHexTable(uip_len, uip_buf);
#endif
// struct uip_eth_hdr {
// struct uip_eth_addr dest;
// struct uip_eth_addr src;
// u16_t type;
// };
// struct uip_eth_addr { // Representation of a 48-bit Ethernet address
// u8_t addr[6];
// };
// http://www.netmanias.com/ko/post/blog/5372/ethernet-ip-tcp-ip/packet-header-ethernet-ip-tcp-ip
// UIP_ETHTYPE_IP(0x0800) : IPv4 Packet(ICMP, TCP, UDP)
uip_arp_ipin();
#ifdef MY_DEBUG
//rprintf("ip in : uip_len = %d, proto = %d\n", uip_len, uip_buf[23]);
//debugPrintHexTable(uip_len, uip_buf+14);
#endif
// uip_arp.c
// #define IPBUF ((struct ethip_hdr *)&uip_buf[0])
// struct ethip_hdr {
// struct uip_eth_hdr ethhdr;
// // IP header
// u8_t vhl,
// tos,
// len[2],
// ipid[2],
// ipoffset[2],
// ttl,
// proto; // ICMP: 1, TCP: 6, UDP: 17
// u16_t ipchksum;
// u16_t srcipaddr[2],
// destipaddr[2];
// }
// if ((IBUF->srcipaddr & uip_netmask) != uip_hostaddr & (uip_netmask)) return;
// uip_arp_update(IPBUF->srcipaddr, &(IPBUF->ethhdr.src));
uip_input();
#ifdef MY_DEBUG
//rprintf("ip out : uip_len = %d, proto = %d\n", uip_len, uip_buf[23]);
//debugPrintHexTable(uip_len, uip_buf+14);
#endif
// ip out packet
// eg ICMP
// uip_len : 84
// source ip <-> destination ip
// type : 8 (Echo (ping) request) -> 0 (Echo (ping) reply)
// uip.h
// #define uip_input() uip_process(UIP_DATA) // UIP_DATA(1) : Tells uIP that there is incoming
// uip_process : The actual uIP function which does all the work.
if(uip_len > 0) {
uip_arp_out();
#ifdef MY_DEBUG
//rprintf("ip out : uip_len = %d, proto = %d\n", uip_len, uip_buf[23]);
//debugPrintHexTable(uip_len, uip_buf);
#endif
// Destination MAC Address <=> Source MAC Address
// w/o Ethernet CRC
// uip_arp.c
network_send();
// network.c
// if(uip_len <= UIP_LLH_LEN + 40){ // UIP_LLH_LEN : 14
// enc28j60PacketSend(uip_len, (uint8_t *)uip_buf, 0, 0);
// }else{
// enc28j60PacketSend(54, (uint8_t *)uip_buf , uip_len - UIP_LLH_LEN - 40, (uint8_t*)uip_appdata);
// }
}
}else if(BUF->type == htons(UIP_ETHTYPE_ARP)){
// UIP_ETHYPE_ARP(0x0806)
#ifdef MY_DEBUG
//rprintf("arp in : uip_len = %d\n", uip_len);
//debugPrintHexTable(uip_len, uip_buf);
#endif
// arp in : 64 bytes
uip_arp_arpin();
if(uip_len > 0){ // always uip_len > 0
#ifdef MY_DEBUG
//rprintf("ip in : uip_len = %d\n", uip_len);
//debugPrintHexTable(uip_len, uip_buf);
#endif
// arp out : 42 bytes
// 64 - Ethernet_padding(18) - Ethernet_CRC(4)
// Send MAC address <--> Target MAC address
// Send IP address <--> Target IP address
// uip_arp.c
network_send();
}
}
}else if(timer_expired(&periodic_timer)) {
timer_reset(&periodic_timer);
for(i = 0; i < UIP_CONNS; i++) {
uip_periodic(i);
// uip.h
// #define uip_udp_periodic(conn) do { uip_udp_conn = &uip_udp_conns[conn]; \
// uip_process(UIP_UDP_TIMER); } while (0) // UIP_UDP_TIMER : 5
// uip.c; uip_process
// if(flag == UIP_UDP_TIMER) {
// if(uip_udp_conn->lport != 0) {
// uip_conn = NULL;
// uip_sappdata = uip_appdata = &uip_buf[UIP_LLH_LEN + UIP_IPUDPH_LEN];
// uip_len = uip_slen = 0;
// uip_flags = UIP_POLL;
// UIP_UDP_APPCALL();
// goto udp_send;
// }
// } else {
// goto drop;
// }
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}
#if UIP_UDP
for(i = 0; i < UIP_UDP_CONNS; i++) {
uip_udp_periodic(i);
if(uip_len > 0) {
uip_arp_out();
network_send();
}
}
#endif /* UIP_UDP */
if(timer_expired(&arp_timer)) {
timer_reset(&arp_timer);
uip_arp_timer();
}
}
}
return 0;
}
/*******************************************************************
* rprintf_test
*******************************************************************/
void rprintf_test(void)
{
u16 val;
u08 mydata;
u08 mystring[10];
double b;
u08 small;
u16 medium;
u32 big;
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(38400);
// initialize rprintf system
// - use uartSendByte as the output for all rprintf statements
// this will cause all rprintf library functions to direct their
// output to the uart
// - rprintf can be made to output to any device which takes characters.
// You must write a function which takes an unsigned char as an argument
// and then pass this to rprintfInit like this: rprintfInit(YOUR_FUNCTION);
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
while (!(getkey() == 1)) //do the folling block until enter is pressed
{
// print a little intro message so we know things are working
rprintf("\r\nWelcome to rprintf Test!\r\n");
// print single characters
rprintfChar('H');
rprintfChar('e');
rprintfChar('l');
rprintfChar('l');
rprintfChar('o');
// print a constant string stored in FLASH
rprintfProgStrM(" World!");
// print a carriage return, line feed combination
rprintfCRLF();
// note that using rprintfCRLF() is more memory-efficient than
// using rprintf("\r\n"), especially if you do it repeatedly
mystring[0] = 'A';
mystring[1] = ' ';
mystring[2] = 'S';
mystring[3] = 't';
mystring[4] = 'r';
mystring[5] = 'i';
mystring[6] = 'n';
mystring[7] = 'g';
mystring[8] = '!';
mystring[9] = 0; // null termination
// print a null-terminated string from RAM
rprintfStr(mystring);
rprintfCRLF();
// print a section of a string from RAM
// - start at index 2
// - print 6 characters
rprintfStrLen(mystring, 2, 6);
rprintfCRLF();
val = 24060;
mydata = 'L';
// print a decimal number
rprintf("This is a decimal number: %d\r\n", val);
// print a hex number
rprintf("This is a hex number: %x\r\n", mydata);
// print a character
rprintf("This is a character: %c\r\n", mydata);
// print hex numbers
small = 0x12; // a char
medium = 0x1234; // a short
big = 0x12345678; // a long
rprintf("This is a 2-digit hex number (char) : ");
rprintfu08(small);
rprintfCRLF();
rprintf("This is a 4-digit hex number (short): ");
rprintfu16(medium);
rprintfCRLF();
rprintf("This is a 8-digit hex number (long) : ");
rprintfu32(big);
rprintfCRLF();
// print a formatted decimal number
// - use base 10
// - use 8 characters
// - the number is signed [TRUE]
// - pad with '.' periods
rprintf("This is a formatted decimal number: ");
rprintfNum(10, 8, TRUE, '.', val);
rprintfCRLF();
b = 1.23456;
// print a floating point number
// use 10-digit precision
// NOTE: TO USE rprintfFloat() YOU MUST ENABLE SUPPORT IN global.h
// use the following in your global.h: #define RPRINTF_FLOAT
rprintf("This is a floating point number: ");
rprintfFloat(8, b);
rprintfCRLF();
}
}
void test_oscope(void)
{
int se0,se1,se2,se3,se4,se5,se6,se7,se8;
int se9,se10,se11,se12,se13,se14,se15;
LED_on();
if (button_pressed())
LED_off();
//pulse each fast for oscope testing
//right row
servo(PORTC,0,360);
servo(PORTC,1,360);
servo(PORTC,2,360);
servo(PORTC,3,360);
servo(PORTC,4,360);
servo(PORTC,5,360);
servo(PORTC,6,360);
servo(PORTC,7,360);
servo(PORTJ,6,360);
servo(PORTA,0,360);
servo(PORTA,1,360);
servo(PORTA,2,360);
servo(PORTA,3,360);
servo(PORTA,4,360);
servo(PORTA,5,360);
servo(PORTA,6,360);
servo(PORTA,7,360);
//left row
servo(PORTE,2,360);
servo(PORTE,3,360);
servo(PORTE,4,360);
servo(PORTE,5,360);
servo(PORTE,6,360);
servo(PORTE,7,360);
servo(PORTH,2,360);
servo(PORTH,3,360);
servo(PORTH,4,360);
servo(PORTH,5,360);
servo(PORTH,6,360);
//top row
se0=a2dConvert8bit(0);
se1=a2dConvert8bit(1);
se2=a2dConvert8bit(2);
se3=a2dConvert8bit(3);
se4=a2dConvert8bit(4);
se5=a2dConvert8bit(5);
se6=a2dConvert8bit(6);
se7=a2dConvert8bit(7);
se8=a2dConvert8bit(8);
se9=a2dConvert8bit(9);
se10=a2dConvert8bit(10);
se11=a2dConvert8bit(11);
se12=a2dConvert8bit(12);
se13=a2dConvert8bit(13);
se14=a2dConvert8bit(14);
se15=a2dConvert8bit(0x0f);//15
rprintfInit(uart1SendByte);
rprintf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d \r\n",se0,se1,se2,se3,se4,se5,se6,se7);
rprintf("8:%d 9:%d 10:%d 11:%d 12:%d 13:%d 14:%d 15:%d \r\n",se8,se9,se10,se11,se12,se13,se14,se15);
rprintfInit(uart0SendByte);
rprintf(" 0");
rprintfInit(uart2SendByte);
rprintf(" 2");
rprintfInit(uart3SendByte);
rprintf(" 3");
delay_cycles(65500);
}
// Process incoming characters
void gaitDesignerProcess(GAIT_DESIGNER* gait){
int b = __uartGetByte(gait->uart);
if(b==-1) return; // No characters at all
uint8_t inx = gait->msgInx;
uint8_t* buffer = gait->buffer;
Writer old = rprintfInit(gait->uart->writer);
// Process all received characters
while(b!=-1){
uint8_t servo;
int8_t percent;
uint8_t c = b & 0xff;;
// _uartSendByte(gait->uart,c);
if(c == '\r'){
// ignore it
}else if(c=='\n'){
buffer[inx] = '\0';
// now process msg
if(buffer[0]=='G' && inx>=3 && inx%2 ==1){
// A group message,
inx=0;
for(servo=0; servo < gait->num_actuators && buffer[inx+1];servo++){
inx++;
int8_t percent = (hexDigit(buffer,inx) << 4);
inx++;
percent |= hexDigit(buffer,inx);
setSpeed(gait,servo,percent);
}
}else if(buffer[0]=='N' && inx==1){
// Reply with number of servos
rprintf("#n%d\r\n",gait->num_actuators);
}else if(buffer[0]=='C'){
// Get config ie: C0
// Reply with: c0,center,range
servo=0;
inx=1;
while(buffer[inx]>='0' && buffer[inx]<='9'){
servo*=10;
servo+= (buffer[inx++] & 15);
}
SERVO* theServo = (SERVO*)getEntry(gait, servo);
rprintf("#c%d,%d,%d\r\n",servo,theServo->center_us,theServo->range_us);
}else if(buffer[0]=='S'){
// Single servo cmd <inx>,<speed>
boolean neg = FALSE;
servo=0;
percent=0;
inx=1;
while(buffer[inx]>='0' && buffer[inx]<='9'){
servo*=10;
servo+= (buffer[inx++] & 15);
}
if(buffer[inx++]==','){
if(buffer[inx]=='-'){
inx++;
neg=TRUE;
}
while(buffer[inx]){
percent*=10;
percent += (buffer[inx++] & 15);
}
if(neg){
percent *= -1;
}
setSpeed(gait,servo,percent);
}
}
inx=0;
}else if (c=='#'){
inx = 0;
}else{
buffer[inx++] = b;
}
b = __uartGetByte(gait->uart);
}
gait->msgInx = inx;
rprintfInit(old);
}
