int main (void)
{
int i=0;
//Setup oscillator/ports/pins first
setup_oscillator();
setup_ports();
setup_peripheral_pin_select();
setup_interrupt_priorities();
//Blinking HB LED and Timestamp
#ifdef USE_DIO
setup_dio();
#endif
//ToDo Used?
#ifdef USE_TIMER1
setup_timer1(); //do before setup_pwm
#endif //ToDo: there was a missing #endif, maybe that's why disabling Timer1 was breaking the code!
//Motor control PWM
#ifdef USE_PWM
setup_pwm();
#endif
//Brushless motor
#ifdef USE_BLDC
setup_bldc();
#endif
//Torque PID and Current PID
#ifdef USE_CONTROL
setup_control();
#endif
//Analog to Digital
#ifdef USE_ADC
setup_adc();
#endif
//Torso external ADC
#ifdef USE_ADC_SPI
setup_adc_spi();
#endif
//Current measurment, control and limitation
#ifdef USE_CURRENT
setup_current();
#endif
//ToDo: Remvove?
#ifdef USE_TIMER3
setup_timer3();
#endif
//VerteX SPI Encoder
#ifdef USE_ENCODER_VERTX
setup_vertx();
#endif
//Torso motor brake
#ifdef USE_BRAKE
setup_brake();
#endif
//EtherCAT Communication
#ifdef USE_ETHERCAT
while (!eeprom_loaded()) //Wait until ESC is ready
SetHeartbeatLED;
setup_ethercat();
ClrHeartbeatLED;
#endif
setup_interrupt_priorities();
while(1)
{
if (i++%20001==0)
{
ToggleHeartbeatLED();
}
#if defined USE_ETHERCAT
step_ethercat();
#endif
}
}
/**
* Allocate and initialize memory, buffers, pages, PWM, DMA, and GPIO.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns 0 on success, -1 otherwise.
*/
int ws2811_init(ws2811_t *ws2811)
{
ws2811_device_t *device = NULL;
int chan;
// Zero mbox; non-zero values indicate action needed on cleanup
memset(&mbox, 0, sizeof(mbox));
ws2811->device = malloc(sizeof(*ws2811->device));
if (!ws2811->device)
{
return -1;
}
device = ws2811->device;
// Determine how much physical memory we need for DMA
mbox.size = PWM_BYTE_COUNT(max_channel_led_count(ws2811), ws2811->freq) +
+ sizeof(dma_cb_t);
// Round up to page size multiple
mbox.size = (mbox.size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1);
// Use the mailbox interface to request memory from the VideoCore
// We specifiy (-1) for the handle rather than calling mbox_open()
// so multiple users can share the resource.
mbox.handle = -1; // mbox_open();
mbox.mem_ref = mem_alloc(mbox.handle, mbox.size, PAGE_SIZE,
board_info_sdram_address() == 0x40000000 ? 0xC : 0x4);
if (mbox.mem_ref == (unsigned) ~0)
{
return -1;
}
mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
if (mbox.bus_addr == (unsigned) ~0)
{
mem_free(mbox.handle, mbox.size);
return -1;
}
mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), mbox.size);
// Initialize all pointers to NULL. Any non-NULL pointers will be freed on cleanup.
device->pwm_raw = NULL;
device->dma_cb = NULL;
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811->channel[chan].leds = NULL;
}
// Allocate the LED buffers
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811_channel_t *channel = &ws2811->channel[chan];
channel->leds = malloc(sizeof(ws2811_led_t) * channel->count);
if (!channel->leds)
{
goto err;
}
memset(channel->leds, 0, sizeof(ws2811_led_t) * channel->count);
}
device->dma_cb = (dma_cb_t *)mbox.virt_addr;
device->pwm_raw = (uint8_t *)mbox.virt_addr + sizeof(dma_cb_t);
pwm_raw_init(ws2811);
memset((dma_cb_t *)device->dma_cb, 0, sizeof(dma_cb_t));
// Cache the DMA control block bus address
device->dma_cb_addr = addr_to_bus(device->dma_cb);
// Map the physical registers into userspace
if (map_registers(ws2811))
{
goto err;
}
// Initialize the GPIO pins
if (gpio_init(ws2811))
{
unmap_registers(ws2811);
goto err;
}
// Setup the PWM, clocks, and DMA
if (setup_pwm(ws2811))
{
unmap_registers(ws2811);
goto err;
}
return 0;
err:
ws2811_cleanup(ws2811);
return -1;
}
int main(void)
{
//========= Peripheral Enable and Setup ===========//
setup_LCD(); //LCD Screen setup
setup_tmp36(); //TMP36 ADC thermometer setup
setup_GPS(); //GPS Pin Setup
setup_microSD(); //MicroSD Pin Setup
setupTMP102();
setup_PID(&pid);
setupBMS();
setup_pwm();
//Setup the buttons
setup_buttonInterrupts();
sleepEnablePeripherals();
setup_timerInterrupt();
//========= Peripheral enable and run ============//
enable_LCD(); //Start LCD Commmunication
enable_GPS(); //Start GPS Communication
clearDisplay(); //Refresh Display
while(1)
{
//Listen for the GPS Data
listen_GPS();
//Open the datalog file for writing
open_datalog();
//Obtain vehicle speed
char *velocity;
char velocityArray[] = "000";
//velocity = velocityArray;
velocity = getVelocity();
//Convert string to int
int speedInteger = atoi(velocity);
//Convert knots to mph
speedInteger = 1.15 * speedInteger;
//Return to string
sprintf(velocity, "%i", speedInteger);
// velocity = "50";
selectLineOne();
//Show velocity
putPhrase("V:");
putPhrase(velocity);
//Analog Temperature Sensor
int anag_tempValue = get_analog_temp();
sprintf(anag_temp, "%i", anag_tempValue);
//Digital Temperature Sensor
int digi_tempValue = getTemperature();
sprintf(digi_temp, "%i", digi_tempValue);
//Get BMS Level
char load[] = "89";
char *load_value;
load_value = load;
//-------BMS Communication------
sendStartSequence();
getDataString();
load_value = getLoad();
//------------------------------
//If Cruise Control is on
if(enableSys)
{
//If initialized
if(obtain_speed)
{ //Debouncing
//SysCtlDelay(533333);
obtain_speed = 0;
clearDisplay();
//Get set velocity
set_velocity = 10;
}
//If driver increases set speed
if(incr_speed)
{
//SysCtlDelay(533333);
incr_speed = 0;
set_velocity += 1;
}
//if driver decresaes set speed
else if(decr_speed)
{
//SysCtlDelay(533333);
decr_speed = 0;
set_velocity -= 1;
}
//==============PID Portion of the Main Loop ==============//
float process_value = atof(velocity); //Convert String to Int
float error = set_velocity - process_value; //Calculate error
float u_t = UpdatePID(&pid, error, 1.0); //Feed error to the PID
uint32_t duty_cycle = u_t*scaleFactor;
if(duty_cycle < 950){
duty_cycle = 50;
}
else if(duty_cycle > 0){
duty_cycle = 950;
}
pwm_out(1000, duty_cycle); //Scale and output the PID output
//====================================================//
//Show other essentials to the LCD
putPhrase("mph/");
char set_point[5];
sprintf(set_point, "%imph", (int) set_velocity);
putPhrase(set_point);
putPhrase(" ON"); //Cruise control on
}
else
{
//Spacer
putPhrase("mph ");
putPhrase("Standby ");
}
//Select bottom line
selectLineTwo();
//Display analog and digital temperatures
putPhrase("T:");
putPhrase(anag_temp);
putPhrase("/");
putPhrase(digi_temp);
putPhrase("C ");
//Show Load level
putPhrase("B:");
putPhrase(load_value);
putPhrase("%");
write_datalog(velocity, "ddmm.mmmmmmX", "ddmm.mmmmmX", getTime(), anag_temp, digi_temp, load_value);
close();
//Put system in low power mode
SysCtlSleep();
}
}
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
}
}
int main(void)
{ char key;
// Map the I/O sections
setup_io();
// Set ALL GPIO pins to the required mode
setup_gpio();
// Set up PWM module
setup_pwm();
// Setup the SPI
setup_spi();
// We don't touch the UART for now
//
// Here your main program can start
//
do {
printf(" l/L : Walk the LEDS\n");
printf(" b/B : Show buttons\n");
printf(" m/M : Control the motor\n");
printf(" a/A : Read the ADC values\n");
printf(" c/C : ADC => Motor\n");
printf("( D : Set the DAC values\n");
printf(" q/Q : Quit program\n");
key = getchar();
switch (key)
{
case 'l':
case 'L':
quick_led_demo();
break;
case 'b':
case 'B':
quick_buttons_demo();
break;
case 'm':
case 'M':
quick_pwm_demo();
break;
case 'a':
case 'A':
quick_adc_demo();
break;
case 'c':
case 'C':
adc_pwm_demo();
break;
case 0x0A:
case 0x0D:
// ignore CR/LF
break;
default:
printf("???\n");
}
} while (key!='q' && key!='Q');
// make sure everything is off!
leds_off();
pwm_off();
restore_io();
return 0;
} // main
/**
* Allocate and initialize memory, buffers, pages, PWM, DMA, and GPIO.
*
* @param ws2811 ws2811 instance pointer.
*
* @returns 0 on success, -1 otherwise.
*/
int ws2811_init(ws2811_t *ws2811)
{
ws2811_device_t *device;
const rpi_hw_t *rpi_hw;
int chan;
ws2811->rpi_hw = rpi_hw_detect();
if (!ws2811->rpi_hw)
{
return -1;
}
rpi_hw = ws2811->rpi_hw;
ws2811->device = malloc(sizeof(*ws2811->device));
if (!ws2811->device)
{
return -1;
}
device = ws2811->device;
// Determine how much physical memory we need for DMA
device->mbox.size = PWM_BYTE_COUNT(max_channel_led_count(ws2811), ws2811->freq) +
sizeof(dma_cb_t);
// Round up to page size multiple
device->mbox.size = (device->mbox.size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1);
device->mbox.handle = mbox_open();
if (device->mbox.handle == -1)
{
return -1;
}
device->mbox.mem_ref = mem_alloc(device->mbox.handle, device->mbox.size, PAGE_SIZE,
rpi_hw->videocore_base == 0x40000000 ? 0xC : 0x4);
if (device->mbox.mem_ref == 0)
{
return -1;
}
device->mbox.bus_addr = mem_lock(device->mbox.handle, device->mbox.mem_ref);
if (device->mbox.bus_addr == (uint32_t) ~0UL)
{
mem_free(device->mbox.handle, device->mbox.size);
return -1;
}
device->mbox.virt_addr = mapmem(BUS_TO_PHYS(device->mbox.bus_addr), device->mbox.size);
// Initialize all pointers to NULL. Any non-NULL pointers will be freed on cleanup.
device->pwm_raw = NULL;
device->dma_cb = NULL;
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811->channel[chan].leds = NULL;
}
// Allocate the LED buffers
for (chan = 0; chan < RPI_PWM_CHANNELS; chan++)
{
ws2811_channel_t *channel = &ws2811->channel[chan];
channel->leds = malloc(sizeof(ws2811_led_t) * channel->count);
if (!channel->leds)
{
goto err;
}
memset(channel->leds, 0, sizeof(ws2811_led_t) * channel->count);
if (!channel->strip_type)
{
channel->strip_type=WS2811_STRIP_RGB;
}
}
device->dma_cb = (dma_cb_t *)device->mbox.virt_addr;
device->pwm_raw = (uint8_t *)device->mbox.virt_addr + sizeof(dma_cb_t);
pwm_raw_init(ws2811);
memset((dma_cb_t *)device->dma_cb, 0, sizeof(dma_cb_t));
// Cache the DMA control block bus address
device->dma_cb_addr = addr_to_bus(device, device->dma_cb);
// Map the physical registers into userspace
if (map_registers(ws2811))
{
goto err;
}
// Initialize the GPIO pins
if (gpio_init(ws2811))
{
unmap_registers(ws2811);
goto err;
}
// Setup the PWM, clocks, and DMA
if (setup_pwm(ws2811))
{
unmap_registers(ws2811);
goto err;
}
return 0;
err:
ws2811_cleanup(ws2811);
return -1;
}
void main(void){
//------------------------------------------------------------------------------
// Main Program
// This is the main routine for the program. Execution of code starts here.
// The operating system is Back Ground Fore Ground.
//
//------------------------------------------------------------------------------
init_ports();
Init_Clocks();
Init_Conditions();
init_timers();
five_msec_delay(QUARTER_SECOND);
Init_LCD();
setup_sw_debounce();
init_adc();
P1OUT |= IR_LED;
init_serial_uart();
WDTCTL = WDTPW + WDTHOLD;
setup_pwm();
set_motor_speed(R_FORWARD, PWM_RES);
set_motor_speed(L_FORWARD, PWM_RES);
unsigned int time_sequence = START_VAL; // counter for switch loop
unsigned int previous_count = START_VAL; // automatic variable for
// comparing timer_count
unsigned int display_count = START_VAL;
is_follow_running = FALSE;
//------------------------------------------------------------------------------
// Begining of the "While" Operating System
//------------------------------------------------------------------------------
while(ALWAYS) { // Can the Operating system run
if(get_timer_count() > display_count + QUARTER_SECOND)
{
display_count = get_timer_count();
Display_Process();
time_sequence = START_VAL;
}
update_switches(); // Check for switch state change
update_menu();
if(is_follow_running)
run_follow();
if(uca0_is_message_received())
{
BufferString message = uca0_read_buffer(TRUE);
receive_command(message.head + message.offset);
}
if(uca1_is_message_received())
{
update_menu();
BufferString message = uca1_read_buffer(TRUE);
uca0_transmit_message(message.head, message.offset);
if(find(WIFI_COMMAND_SYMBOL, message))
{
receive_command(message.head + message.offset);
}
if(find(LOST_WIFI_COMMAND_SYMBOL, message))
{
receive_command(CONNECT_NCSU);
}
}
if(time_sequence > SECOND_AND_A_QUARTER)
time_sequence = START_VAL;
unsigned int current_timer_count = get_timer_count();
if(current_timer_count > previous_count)
{
previous_count = current_timer_count % UINT_16_MAX;
time_sequence++;
}
}
//------------------------------------------------------------------------------
}
int main(void) {
LPC_GPIO2->DIR |= (1<<10);
/* PLL is already setup */
SystemCoreClockUpdate();
/* Blue LED Set as output */
/* Relay EN Set as output */
LPC_GPIO1->DIR |= (1<<5);
uartInit(115200);
puts("Sys Initted\n");
puts("uart Initted\n");
initSwitch();
puts("Switches Initted\n");
SysTick_Config(SystemCoreClock/1000);
puts("SysTick Initted\n");
setup_pwm(1000,4092);
puts("PWM Initted\n");
setup_GPIO_INT();
puts("GPIO INT Initted\n");
filt_adc_init(450);
delay_ms(500);
puts("Filt ADC Initted\n");
while(1)
{
while(next_run > msTicks)
__WFI();
next_run= msTicks + 10;
if ((msTicks - last_motion ) >
(MAX(W_POWER_OFF_TIME_MS + RAMP_DOWN_TIME_MS,
RGB_POWER_OFF_TIME_MS + RAMP_DOWN_TIME_MS ) + 2000))
{
shut_off_supply();
transfer_to_sleep();
turn_on_supply();
/* Wait for the power to actually come on before starting the ramp up */
}
// WW_SWITCH_HANDLER
// debounce for 20ms, since we check for the absolute val if the user
// holds the switch forever it will only trigger the service once
if(getSwitch(WW_SWITCH) == 0)
{
if(ww_sw_asserted == 5)
{
puts("WW SWITCH PRESSED\n");
white_on = !white_on;
white_scale = 0;
}
ww_sw_asserted += 1;
}
else
ww_sw_asserted = 0;
// OFF_SWITCH_HANDLER
// debounce for 20ms, since we check for the absolute val if the user
// holds the switch forever it will only trigger the service once
if(getSwitch(OFF_SWITCH) == 0)
{
if(off_sw_asserted == 5)
{
puts("OFF SWITCH PRESSED\n");
white_on = 0;
white_scale = 0;
rgb_on = 0;
rgb_scale = 0;
}
off_sw_asserted += 1;
}
else
off_sw_asserted = 0;
// RGB_SWITCH_HANDLER
// debounce for 20ms, since we check for the absolute val if the user
// holds the switch forever it will only trigger the service once
if(getSwitch(RGB_SWITCH) == 0)
{
if(rgb_sw_asserted == 5)
{
puts("RGB SWITCH PRESSED\n");
rgb_on = !rgb_on;
rgb_scale = 0;
}
if(rgb_sw_asserted == 100)
{
puts("RGB SWITCH HELD\n");
rgb_on = 2;
}
if(rgb_sw_asserted == 500)
{
puts("RGB SWITCH LONG HELD\n");
rgb_on = 3;
}
rgb_sw_asserted += 1;
}
else
rgb_sw_asserted = 0;
/* This handles the WW LEDs */
if (((msTicks - last_motion ) > W_POWER_OFF_TIME_MS) && white_on)
{
white_scale = RAMP_DOWN_TIME_MS - ((msTicks - last_motion) - W_POWER_OFF_TIME_MS);
if ((msTicks - last_motion) > (W_POWER_OFF_TIME_MS + RAMP_DOWN_TIME_MS))
white_scale = 0;
}
if ( white_scale < RAMP_DOWN_TIME_MS && white_on && ((msTicks - last_motion ) < W_POWER_OFF_TIME_MS))
{
white_scale = msTicks - last_motion;
/* Check just in case we get held up and skip the 1000th ms after motion */
if ( white_scale > RAMP_DOWN_TIME_MS)
white_scale = RAMP_DOWN_TIME_MS;
}
if (white_scale != RAMP_DOWN_TIME_MS)
{
setWW( (white_scale * getADCVal(WW_POT)) / RAMP_DOWN_TIME_MS );
setWW2( (white_scale * getADCVal(WW_POT)) / RAMP_DOWN_TIME_MS );
}
else
{
setWW(getADCVal(WW_POT));
setWW2(getADCVal(WW_POT));
}
/* This handles the RGB LEDs */
if (((msTicks - last_motion ) > RGB_POWER_OFF_TIME_MS) && rgb_on)
{
rgb_scale = RAMP_DOWN_TIME_MS - ((msTicks - last_motion) - RGB_POWER_OFF_TIME_MS);
if ((msTicks - last_motion) > (RGB_POWER_OFF_TIME_MS + RAMP_DOWN_TIME_MS))
rgb_scale = 0;
}
if ( rgb_scale < RAMP_DOWN_TIME_MS && rgb_on && ((msTicks - last_motion ) < RGB_POWER_OFF_TIME_MS))
{
rgb_scale = msTicks - last_motion;
/* Check just in case we get held up and skip the 1000th ms after motion */
if ( rgb_scale > RAMP_DOWN_TIME_MS)
rgb_scale = RAMP_DOWN_TIME_MS;
}
if (rgb_on == 1 || rgb_on == 0)
{
if (rgb_scale != RAMP_DOWN_TIME_MS)
setRGB((rgb_scale * getADCVal(RED_POT)) / RAMP_DOWN_TIME_MS,
(rgb_scale * getADCVal(GREEN_POT)) / RAMP_DOWN_TIME_MS,
(rgb_scale * getADCVal(BLUE_POT)) / RAMP_DOWN_TIME_MS );
else
setRGB(getADCVal(RED_POT), getADCVal(GREEN_POT), getADCVal(BLUE_POT));
}
else if (rgb_on == 2)
{
uint32_t R,G,B;
h2rgb((msTicks)%(H2RGB_OUT_RANGE*6), &R, &G, &B);
if (rgb_scale != RAMP_DOWN_TIME_MS)
setRGB((rgb_scale * R) / RAMP_DOWN_TIME_MS,
(rgb_scale * G) / RAMP_DOWN_TIME_MS,
(rgb_scale * B) / RAMP_DOWN_TIME_MS );
else
setRGB(R, G, B);
}
else if (rgb_on == 3)
{
uint32_t R,G,B;
if (rgb_mode_3_next_change < msTicks)
{
rgb_mode_3_next_change = msTicks + (getADCVal(RED_POT)/4);
rgb_mode_3_color += 2;
if(rgb_mode_3_color >=6)
rgb_mode_3_color = 0;
}
h2rgb((H2RGB_OUT_RANGE * rgb_mode_3_color), &R, &G, &B);
if (rgb_scale != RAMP_DOWN_TIME_MS)
setRGB((rgb_scale * R) / RAMP_DOWN_TIME_MS,
(rgb_scale * G) / RAMP_DOWN_TIME_MS,
(rgb_scale * B) / RAMP_DOWN_TIME_MS );
else
setRGB(R, G, B);
}
}
}
/*********************************************************************
Function: int main(void)
PreCondition: None.
Input: None.
Output: None.
Side Effects: None.
Overview: main function of the application. Peripherals are
initialized.
Note: None.
********************************************************************/
int main(void)
{
int cs;
// vars used for detection of incremental motion
unsigned short new_cmd,last_cmd, new_fb,last_fb;
setup_io(); // make all i/o pins go the right dir
STATUS_LED = 0; // led on
setup_uart(); // setup the serial interface to the PC
setup_TMR1(); // set up 1ms timer
IEC0bits.T1IE = 1; // Enable interrupts for timer 1
// needed for delays in following routines
// 1/2 seconds startup delay
timer_test = 5000;
while ( timer_test );
printf("\r\nPowerup..i/o...uart...timer...");
setup_pwm(); // start analog output
set_pwm(0.0);
printf("pwm...");
init_pid();
printf("pid...");
setup_encoder(); // 16 bit quadrature encoder module setup
printf("encoder...");
setup_capture(); // 2 pins with quadrature cmd from PC
printf("capture...");
printf("done\r\n");
// some junk for the serial channel
printf("%s%s\n\r",CPWRT,VERSION);
STATUS_LED = 1; // led off when init finished
// restore config from eeprom
// Read array named "setupEE" from DataEEPROM and place
// the result into array in RAM named, "setup"
restore_setup();
cs = calc_cksum(sizeof(pid)/sizeof(int),(int*)&pid);
if ( cs )
{
// opps, no valid setup detected
// assume we are starting from a new box
printf("No valid setup found in EEPROM\r\n");
init_pid();
}
else
{
printf("Using setup from eeprom.. ? for help\r\n");
print_tuning();
}
printf("using %fms servo loop interval\r\n",pid.ticksperservo * 0.1);
// BLOCK OF TEST ROUTINES FOR HARDWARE DEBUGGING
// test_pwm_interface(); // play with opa549 hardware
// test_pc_interface(); // echo cmded posn to serial port
// test_pid_interface(); // test pid loop operation
new_cmd = last_cmd = new_fb = last_fb = 0;
while (1)
{
if ( do_servo ) // check and see if timer 1 has asked for servo calcs to be run
{
do_servo = 0;
if (SVO_ENABLE)
{
if ( pid.enable == 0 ) // last loop, servo was off
{
set_pwm( 0.0 );
printf("servo-enabled\r\n>");
pid.enable = 1;
// make sure we dont move on enabling
cmd_posn = POSCNT; // make 16bit incr registers match
pid.command = 0L; // make 32 bit counter match
pid.feedback = 0L;
// make the 1ms loop temps match
new_cmd = last_cmd = new_fb = last_fb = 0;
pid.error_i = 0.0; // reset integrator
}
// we can time the servo cycle calcs by scoping the PID_ACTIVE pin
PID_ACTIVE = 1; // seems to take about 140us
new_cmd = cmd_posn; // grab current cmd from pc
new_fb = POSCNT; // grab current posn from encoder
pid.command += (long int)((short)(new_cmd - last_cmd));
pid.feedback += (long int)((short)(new_fb - last_fb ));
last_cmd = new_cmd;
last_fb = new_fb;
calc_pid();
// check for a drive fault ( posn error > allowed )
if (( pid.maxerror > 0.0 ) &&
( fabs(pid.error) > pid.maxerror ))
{
short temp = SVO_ENABLE;
set_pwm( 0.0 );
while (1) // trap here until svo disabled or pwr cycle
{
// reset integrator as it may have wound up
pid.error_i = 0.0;
printf("drive fault... maxerror exceeded\r\n");
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
if (temp != SVO_ENABLE)
break;
}
}
else
{
set_pwm(pid.output); // update motor drive
}
PID_ACTIVE = 0; // i/o pin for timing pid calcs
}
else
{
if ( pid.enable == 1 ) // last loop servo was active
{
set_pwm( 0.0 );
pid.enable = 0;
printf("servo-disabled\r\n>");
// extra delay keeps us faulted for min 1 sec to let mechanicals settle
STATUS_LED = 1; timer_test = 10000; while ( timer_test );
}
}
}
// look for serial cmds
// doing this while svo is enabled will cause bumps in servo loop
// because of serial i/o time ( unless we get smart and move svo loop
// into an isr )
if ( rxrdy )
process_serial_buffer();
if (pid.limit_state) // show we are at drive limit(error)
STATUS_LED = 0;
else
STATUS_LED = 1;
}
// to keep compiler happy....
return 0;
}
