static void pwm_start( void )
{
int i;
PWM->dis = ALL_CHANNELS;
PWM->mr = 0; /* MCK straight through */
PWM->ier = MY_CHANNELS;
PWM->imr = MY_CHANNELS;
for( i=0; i<POLES; i+=1 ) {
struct pwm_channel *c = PWM->channel + channels[i];
c->cmr = 0;
c->cprd = period;
c->cdty = period/2; /* safe value to start */
}
/* Set up to vector the interrupt */
AIC->svr[ PWMC_ID ] = (uint32_t) pwm_isr;
AIC->iecr = bit( PWMC_ID );
/* Make sure the tables are set up */
pwm_update();
/* Here we go... */
PWM->ena = MY_CHANNELS;
}
static void adc_thread_entry(void *parameter)
{
rt_device_t device;
#ifdef RT_USING_RTGUI
struct rtgui_event_command ecmd;
RTGUI_EVENT_COMMAND_INIT(&ecmd);
ecmd.type = RTGUI_CMD_USER_INT;
ecmd.command_id = ADC_UPDATE;
#else
struct lcd_msg msg;
#endif
device = rt_device_find("adc");
while(1)
{
rt_device_control(device, RT_DEVICE_CTRL_ADC_START, RT_NULL);
rt_device_control(device, RT_DEVICE_CTRL_ADC_RESULT, &adc_value);
pwm_update(adc_value/3);
#ifdef RT_USING_RTGUI
rtgui_thread_send(info_tid, &ecmd.parent, sizeof(ecmd));
#else
msg.type = ADC_MSG;
msg.adc_value = adc_value;
rt_mq_send(&mq, &msg, sizeof(msg));
#endif
rt_thread_delay(20);
}
}
/*##############################################################################
RGB-CTL · 2011 Florian Knodt <*****@*****.**>
TWI-Stuff based on:
USI TWI Slave driver 1.3
Martin Junghans <*****@*****.**>
Markus Schatzl
RGB-Stuff based on:
moodlamp-rf - fnordlicht firmware next generation
Alexander Neumann <*****@*****.**>
Lars Noschinski <*****@*****.**>
Kiu
Mazzoo
Tobias Schneider([email protected])
Soft-PWM - http://www.mikrocontroller.net/articles/Soft-PWM
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License version 2 as
published by the Free Software Foundation.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
For more information on the GPL, please go to:
http://www.gnu.org/copyleft/gpl.html
//# CONFIGURATION ############################################################*/
// TWI Address (LSB must be 0!)
// Define is inside the Makefile
#define SLAVE_ADDR_ATTINY TWI_ADDRESS
//# IMPORT LIBS ##############################################################*/
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <avr/sleep.h>
#include "usiTwiSlave.h"
#ifndef F_CPU
#define F_CPU 8000000UL
#endif
//# MACROS ###################################################################*/
#define uniq(LOW,HEIGHT) ((HEIGHT << 8)|LOW) // Create 16 bit number from two bytes
#define LOW_BYTE(x) (x & 0xff) // Get low byte from 16 bit number
#define HIGH_BYTE(x) ((x >> 8) & 0xff) // Get high byte from 16 bit number
#define sbi(ADDRESS,BIT) ((ADDRESS) |= (1<<(BIT))) // Set bit
#define cbi(ADDRESS,BIT) ((ADDRESS) &= ~(1<<(BIT))) // Clear bit
#define toggle(ADDRESS,BIT) ((ADDRESS) ^= (1<<BIT)) // Toggle bit
#define bis(ADDRESS,BIT) (ADDRESS & (1<<BIT)) // Is bit set?
#define bic(ADDRESS,BIT) (!(ADDRESS & (1<<BIT))) // Is bit clear?
//# GLOBAL VARS ##############################################################*/
uint8_t fade[3] = {0,0,0}; //Color fade delay
uint8_t target[3] = {0,0,0}; //Target color
uint8_t color[3] = {0,0,0}; //Active color
uint16_t colort[3] = {0,0,0}; //Timer compare for active color
volatile uint8_t pwmstate=0; //Used to detect a completed PWM-Cycle
volatile uint16_t pwm_cnt=0; //PWM-Cycle counter
ISR(TIMER1_COMPA_vect) {
TCNT1 = 0;
if (colort[0] <= pwm_cnt)
cbi(PORTB, PB1);
if (colort[1] <= pwm_cnt)
cbi(PORTB, PB3);
if (colort[2] <= pwm_cnt)
cbi(PORTB, PB4);
if (pwm_cnt>=768) {
pwm_cnt=0;
pwmstate=0;
if(colort[0] != 0) sbi(PORTB, PB1);
if(colort[1] != 0) sbi(PORTB, PB3);
if(colort[2] != 0) sbi(PORTB, PB4);
} else
pwm_cnt++;
}
void i2c_process(void) {
//###Process incoming I²C-Data###
//Target Colors
if(twibuffer[0]<=170) target[0] = twibuffer[0];
if(twibuffer[1]<=170) target[1] = twibuffer[1];
if(twibuffer[2]<=170) target[2] = twibuffer[2];
//Fade type
fade[0] = twibuffer[3];
fade[1] = twibuffer[4];
fade[2] = twibuffer[5];
//###Write new values to twibuffer so we can read current status###
twibuffer[6] = color[0];
twibuffer[7] = color[1];
twibuffer[8] = color[2];
twibuffer[9] = LOW_BYTE(colort[0]);
twibuffer[10] = HIGH_BYTE(colort[0]);
twibuffer[11] = LOW_BYTE(colort[1]);
twibuffer[12] = HIGH_BYTE(colort[1]);
twibuffer[13] = LOW_BYTE(colort[2]);
twibuffer[14] = HIGH_BYTE(colort[2]);
}
void pwm_update(void) {
//wait until current pwm-sequence is completed
pwmstate=1;
while(pwmstate != 0) {
set_sleep_mode(SLEEP_MODE_IDLE);
sleep_mode();
}
}
//# MAIN LOOP ################################################################*/
int main(void)
{
/* Red = PB1
Green = PB3
Blue = PB4
*/
uint8_t wait[3]={0,0,0}; //Fading delay temp storage
uint8_t i=0; //Color processing loop temp storage
static const uint16_t timeslot_table[] PROGMEM =
{ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 6, 6, 6, 6, 7, 7, 7, 7, 8, 8, 8, 9, 9, 10, 10, 11, 11, 11, 12, 13, 13, 14, 14, 15, 16, 16, 17, 18, 19, 19, 20, 21, 22, 23, 24, 25, 26, 27, 29, 30, 31, 32, 34, 35, 37, 39, 40, 42, 44, 46, 48, 50, 52, 55, 57, 60, 62, 65, 68, 71, 74, 77, 81, 84, 88, 92, 96, 100, 105, 109, 114, 119, 125, 130, 136, 142, 148, 155, 161, 169, 176, 184, 192, 200, 209, 219, 228, 238, 249, 260, 272, 284, 296, 309, 323, 337, 352, 368, 384, 401, 419, 437, 457, 477, 498, 520, 543, 567, 592, 618, 646, 674, 704, 735, 768 }; //Values 0 - 170
cli(); //Disable interrupts
wdt_enable(WDTO_500MS); //Enable Watchdog
DDRB = 0xFF; //Port = out
PORTB = 0x00; //LEDs off
usiTwiSlaveInit(SLAVE_ADDR_ATTINY); //TWI slave init
sbi(TCCR1,CS10); //Timer without Prescaler
OCR1A = 128; //PWM-Clock
TIMSK |= (1<<OCIE1A); //Enable T1 Compare Interrupt
sei(); //Enable Interrupts
while(1) {
wdt_reset(); //Watchdog reset
i2c_process(); //Read I²C and write current status
//Fading and PWM-programming
for(i=0; i<=2; i++) {
if(target[i] != color[i]) {
if(fade[i] == 0) {
color[i]=target[i];
}else{
if(wait[i] >= fade[i]) {
if(target[i] > color[i]) color[i]++;
else color[i]--;
wait[i]=0;
}else wait[i]++;
}
colort[i]=pgm_read_word(×lot_table[color[i]]);
}
}
wdt_reset();
pwm_update(); //Controller waits there until one PWM-cycle has completed
}
}
//#######################################################################################################
int main(void)
{
DDRD =0b00000000; DDRA =0b00000000;
PORTD =0b11111111; PORTA =0b11111111;
PWM_DDR = 0xFF; // Port als Ausgang
// Timer 1 OCRA1, als variablem Timer nutzen
TCCR1B = 2; // Timer läuft mit Prescaler 8
TIMSK |= (1<<OCIE1A); // Interrupt freischalten
//###################### Watchdog RESET
/*
Konstante Wert TimeOut
WDTO_15MS 0 15 ms
WDTO_30MS 1 30 ms
WDTO_60MS 2 60 ms
WDTO_120MS 3 120 ms
WDTO_250MS 4 250 ms
WDTO_500MS 5 500 s
WDTO_1S 6 1 S
WDTO_2S 7 2 s */
wdt_enable(WDTO_120MS);
sei(); // Interrupts gloabl einschalten
uint8_t zufall[8];
uint8_t i;
while (1)
{
//### Watchdog
wdt_reset();
//###
for (i=0; i<PWM_CHANNELS; i++)
{
if (zufall[i] > rand()%250) {zufall[i] -= rand()%40;} //
else {zufall[i] += rand()%30;}
}
memcpy(pwm_setting, zufall, 8);
pwm_update();
};
return 0;
}
void pwm_init() {
CCP3CONbits.CCP3M = 0b1100; // 0b11xx PWM mode
CCPR3L = 0b00000000; // Duty cycle, high 8 bits
CCP3CONbits.DC3B = 0b00; // Duty cycle, low 2 bits
CCP5CONbits.CCP5M = 0b1100; // 0b11xx PWM mode
CCPR5L = 0b00000000; // Duty cycle, high 8 bits
CCP5CONbits.DC5B = 0b00; // Duty cycle, low 2 bits
dc_step[0] = 0;
dc_step[1] = 0;
duty_cycle[0] = 0;
duty_cycle[1] = 0;
pwm_update();
}
void pwm_enable(unsigned char pwm /* 1-6 */, unsigned long val) {
// bind pwm# to pin
if(pwm==5) {
PCB_PINSEL1 &= ~(3<<pwm_pinshift[pwm-1]);
PCB_PINSEL1 |= 1<<pwm_pinshift[pwm-1];
}
else {
PCB_PINSEL0 &= ~(3<<pwm_pinshift[pwm-1]);
PCB_PINSEL0 |= 2<<pwm_pinshift[pwm-1];
}
// enable pwm
PWM_PCR = (PWM_PCR & ~0x8183) | BIT(8+pwm);
// set new value
pwm_update(pwm, val);
}
int main(void)
{
//variables
int8_t compareValue = 31;
cli();
//Variablen
myInit();
pwm_init();
pwm_update(compareValue);
//lcd ouput:
lcd_put_string_P(FONT_FIXED_8, NORMAL, PSTR("pos(page/column): "));
//currentPage = lcd_get_position_page();
//currentColumn = lcd_get_position_column();
//lcd_put_short(currentPage);
//lcd_put_string_P(FONT_FIXED_8, NORMAL, PSTR("/"));
//lcd_put_short(currentColumn);
lcd_moveto_xy (1, 0);
lcd_set_font(FONT_FIXED_8,NORMAL);
lcd_put_string_P(FONT_FIXED_8, NORMAL, PSTR("PWM Wert: "));
lcd_put_short(compareValue);
while(1)
{
}
return 0;
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
// Initialize the PWM module.
pwm_init();
// Initialize the ADC module.
adc_init();
// Initialize the PID algorithm module.
pid_init();
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
#if ENCODER_ENABLED
// Initialize software I2C to talk with encoder.
swi2c_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Wait until initial position value is ready.
{
int16_t position;
// Get start-up position
#if ENCODER_ENABLED
position=(int16_t) enc_get_position_value();
#else
while (!adc_position_value_is_ready());
position=(int16_t) adc_get_position_value();
#endif
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(position);
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, position);
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
}
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
pwm_enable();
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
uint8_t tick;
int16_t pwm;
int16_t position;
// Is ADC position value ready?
// NOTE: Even when the encoder is enabled, we still need the ADC potmeasurement as the
// clock because that is how the original firmware was written.
tick=adc_position_value_is_ready();
if(tick)
{
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
}
// Get the new position value.
if(tick)
{
position = (int16_t) adc_get_position_value(); // NOTE: For encoder builds, this is the clock: clear the flag
#if ENCODER_ENABLED
} // Always run the encoder (faster PID to PWM loop = better?)
position = (int16_t) enc_get_position_value();
if (position >= 0)
{
#endif
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position, tick);
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
}
// Is a power value ready?
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
// Initialize the PWM module.
pwm_init();
// Initialize the ADC module.
adc_init();
#if ESTIMATOR_ENABLED
// Initialize the state estimator module.
estimator_init();
#endif
#if REGULATOR_MOTION_ENABLED
// Initialize the regulator algorithm module.
regulator_init();
#endif
#if PID_MOTION_ENABLED
// Initialize the PID algorithm module.
pid_init();
#endif
#if IPD_MOTION_ENABLED
// Initialize the IPD algorithm module.
ipd_init();
#endif
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(registers_read_byte(REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Wait until initial position value is ready.
while (!adc_position_value_is_ready());
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(adc_get_position_value());
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
pwm_enable();
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
// Is position value ready?
if (adc_position_value_is_ready())
{
int16_t pwm;
int16_t position;
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
// Get the new position value.
position = (int16_t) adc_get_position_value();
#if ESTIMATOR_ENABLED
// Estimate velocity.
estimate_velocity(position);
#endif
#if PID_MOTION_ENABLED
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position);
#endif
#if IPD_MOTION_ENABLED
// Call the IPD algorithm module to get a new PWM value.
pwm = ipd_position_to_pwm(position);
#endif
#if REGULATOR_MOTION_ENABLED
// Call the state regulator algorithm module to get a new PWM value.
pwm = regulator_position_to_pwm(position);
#endif
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
}
// Is a power value ready?
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_HI, REG_SEEK_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
void remote_controlled(void)
{
/*¨Parts:
-Leds (L)
-Motors (M)
-servos (S)
-complete movement (R) */
char part;
int opt1=0;
int opt2=0;
int opt3=0;
//scanf("%1c%1i%3i%3i",&part,&opt1,&opt2,&opt3);
scanf("%1c%1i%3i",&part,&opt1,&opt2);
switch (part)
{
/* Motors M+#of motor + speed (99 to -99) */
case 'M':
switch (opt1)
{
case 1:
motor1(opt2);
break;
case 2:
motor2(opt2);
break;
case 3:
motor3(opt2);
break;
case 4:
motor4(opt2);
break;
}
break;
case 'R':
move(opt2,opt3);
break;
case 'S':
switch(opt1)
{
case 1:
pwm_update(SERVO_0,opt2);
break;
case 2:
pwm_update(SERVO_1,opt2);
break;
case 3:
pwm_update(SERVO_2,opt2);
break;
case 4:
pwm_update(SERVO_3,opt2);
break;
}
break;
/* Leds, L + # of led + option
L1000 turn off led 1
L1001 turn on led 1
L1002 toggle led 1 */
case 'L':
switch (opt1)
{
case 1:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED1,0);
break;
case 1:
ioport_set_pin_level(LED1,1);
break;
case 2:
ioport_toggle_pin_level(LED1);
break;
}
break;
case 2:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED2,0);
break;
case 1:
ioport_set_pin_level(LED2,1);
break;
case 2:
ioport_toggle_pin_level(LED2);
break;
}
break;
case 3:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED3,0);
break;
case 1:
ioport_set_pin_level(LED3,1);
break;
case 2:
ioport_toggle_pin_level(LED3);
break;
}
break;
case 4:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED4,0);
break;
case 1:
ioport_set_pin_level(LED4,1);
break;
case 2:
ioport_toggle_pin_level(LED4);
break;
}
break;
case 5:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED5,0);
break;
case 1:
ioport_set_pin_level(LED5,1);
break;
case 2:
ioport_toggle_pin_level(LED5);
break;
}
break;
case 6:
switch(opt2)
{
case 0:
ioport_set_pin_level(LED6,0);
break;
case 1:
ioport_set_pin_level(LED6,1);
break;
case 2:
ioport_toggle_pin_level(LED6);
break;
}
break;
}
// case 'M':
// break;
//
// case 'S':
// break;
//
// case 'R':
// break;
}
}
int main (void)
{
ioport_set_pin_dir(IO_1,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_2,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_3,IOPORT_DIR_OUTPUT);
ioport_set_pin_dir(IO_4,IOPORT_DIR_OUTPUT);
init_octopus();
initial_animation();
pwm_ini(SERVO_0);
pwm_update(SERVO_0,0);
delay_ms(1000);
pwm_update(SERVO_0,500);
delay_ms(1000);
pwm_update(SERVO_0,1000);
char aux;
char aux2;
int dimmer=0;
for(;;)
{
if(bluetooth_is_rx_complete())
{
scanf("%1c",&aux);
switch (aux)
{
case 'C':
scanf("%1c",&aux);
if (aux=='1')
{
led1(ON);
ioport_set_pin_level(IO_1,ON);
}
else
{
led1(OFF);
ioport_set_pin_level(IO_1,OFF);
}
break;
case 'T':
scanf("%1c",&aux);
if (aux=='1')
{
led2(ON);
ioport_set_pin_level(IO_2,ON);
}
else
{
led2(OFF);
ioport_set_pin_level(IO_2,OFF);
}
break;
case 'D':
scanf("%i",&dimmer);
break;
case 'O':
scanf("%1c",&aux);
scanf("%1c",&aux);
scanf("%1c",&aux2);
if(aux == '1')
{
if(aux2=='1')
{
led3(ON);
ioport_set_pin_level(IO_3,ON);
}
else
{
led3(OFF);
ioport_set_pin_level(IO_3,OFF);
}
}
else
{
if(aux2=='1')
{
led4(ON);
ioport_set_pin_level(IO_4,ON);
}
else
{
led4(OFF);
ioport_set_pin_level(IO_4,OFF);
}
}
break;
}
}
dimmer=dimmer*10;
pwm_update(SERVO_0,dimmer);
}
}
void pwm_fade(uint8 index, int8 step) {
dc_step[index] = step;
pwm_update();
}
/** Main (and infinite) loop. */
static void
main_loop (void)
{
while (1)
{
/* Wait until next cycle. */
timer_wait ();
/* Update chrono. */
chrono_update ();
/* Is match over? */
if (chrono_is_match_over ())
{
/* Power off doors. */
pwm_set (BOT_PWM_DOOR_FRONT_BOTTOM, 0);
pwm_set (BOT_PWM_DOOR_FRONT_TOP, 0);
pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, 0);
pwm_set (BOT_PWM_DOOR_BACK_TOP, 0);
pwm_set (BOT_PWM_CLAMP, 0);
/* End it and block here indefinitely. */
chrono_end_match (42);
return;
}
/* Handle commands from UART. */
while (uart0_poll ())
proto_accept (uart0_getc ());
/* Update IO modules. */
pwm_update ();
contact_update ();
pawn_sensor_update ();
if (usdist_update ())
{
position_t robot_pos;
asserv_get_position (&robot_pos);
main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos);
move_obstacles_update ();
simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0);
}
/* Update AI modules. */
logistic_update ();
path_decay ();
/* Only manage events if slaves are synchronised. */
if (twi_master_sync ())
main_event_to_fsm ();
/* Send stats if requested. */
if (main_stats_asserv_ && !--main_stats_asserv_cpt_)
{
/* Get current position */
position_t cur_pos;
asserv_get_position (&cur_pos);
/* Send stats */
proto_send3w ('A', cur_pos.v.x, cur_pos.v.y, cur_pos.a);
/* Reset stats counter */
main_stats_asserv_cpt_ = main_stats_asserv_;
}
if (main_stats_contact_ && !--main_stats_contact_cpt_)
{
proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24);
main_stats_contact_cpt_ = main_stats_contact_;
}
if (main_stats_codebar_ && !--main_stats_codebar_cpt_)
{
proto_send2b ('B', codebar_get (DIRECTION_FORWARD),
codebar_get (DIRECTION_BACKWARD));
main_stats_codebar_cpt_ = main_stats_codebar_;
}
if (main_stats_usdist_ && !--main_stats_usdist_cpt_)
{
proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2],
usdist_mm[3]);
main_stats_usdist_cpt_ = main_stats_usdist_;
}
}
}
int main (void)
{
// Configure pins to the default states.
config_pin_defaults();
// Initialize the watchdog module.
watchdog_init();
// First, initialize registers that control servo operation.
registers_init();
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Initialize the PWM module.
pwm_init();
#endif
#if STEP_ENABLED
// Initialise the stepper motor
step_init();
#endif
// Initialize the ADC module.
adc_init();
// Initialise the Heartbeart
heartbeat_init();
// Initialize the PID algorithm module.
pid_init();
#if CURVE_MOTION_ENABLED
// Initialize curve motion module.
motion_init();
#endif
// Initialize the power module.
power_init();
#if PULSE_CONTROL_ENABLED
pulse_control_init();
#endif
#if BACKEMF_ENABLED
// Initialise the back emf module
backemf_init();
#endif
#if ALERT_ENABLED
//initialise the alert registers
alert_init();
#endif
// Initialize the TWI slave module.
twi_slave_init(banks_read_byte(POS_PID_BANK, REG_TWI_ADDRESS));
// Finally initialize the timer.
timer_set(0);
// Enable interrupts.
sei();
// Trigger the adc sampling hardware
adc_start(ADC_CHANNEL_POSITION);
// Wait until initial position value is ready.
while (!adc_position_value_is_ready());
#if CURVE_MOTION_ENABLED
// Reset the curve motion with the current position of the servo.
motion_reset(adc_get_position_value());
#endif
// Set the initial seek position and velocity.
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, adc_get_position_value());
registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);
// XXX Enable PWM and writing. I do this for now to make development and
// XXX tuning a bit easier. Constantly manually setting these values to
// XXX turn the servo on and write the gain values get's to be a pain.
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
pwm_enable();
#endif
#if STEP_ENABLED
step_enable();
#endif
registers_write_enable();
// This is the main processing loop for the servo. It basically looks
// for new position, power or TWI commands to be processed.
for (;;)
{
static uint8_t emf_motor_is_coasting = 0;
// Is the system heartbeat ready?
if (heartbeat_is_ready())
{
static int16_t last_seek_position;
static int16_t wait_seek_position;
static int16_t new_seek_position;
// Clear the heartbeat flag
heartbeat_value_clear_ready();
#if PULSE_CONTROL_ENABLED
// Give pulse control a chance to update the seek position.
pulse_control_update();
#endif
#if CURVE_MOTION_ENABLED
// Give the motion curve a chance to update the seek position and velocity.
motion_next(10);
#endif
// General call support
// Check to see if we have the wait flag enabled. If so save the new position, and write in the
// old position until we get the move command
if (general_call_enabled())
{
//we need to wait for the go command before moving
if (general_call_wait())
{
// store the new position, but let the servo lock to the last seek position
wait_seek_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
if (wait_seek_position != last_seek_position) // do we have a new position?
{
new_seek_position = wait_seek_position;
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, last_seek_position);
}
}
last_seek_position = registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
//check to make sure that we can start the move.
if (general_call_start() ||
( registers_read_byte(REG_GENERAL_CALL_GROUP_START) == banks_read_byte(CONFIG_BANK, REG_GENERAL_CALL_GROUP)))
{
// write the new position with the previously saved position
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, new_seek_position);
general_call_start_wait_reset(); // reset the wait flag
general_call_start_reset(); // reset the start flag
}
}
#if BACKEMF_ENABLED
// Quick and dirty check to see if pwm is active. This is done to make sure the motor doesn't
// whine in the audible range while idling.
uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
if (pwm_a || pwm_b)
{
// Disable PWM
backemf_coast_motor();
emf_motor_is_coasting = 1;
}
else
{
// reset the back EMF value to 0
banks_write_word(INFORMATION_BANK, REG_BACKEMF_HI, REG_BACKEMF_LO, 0);
emf_motor_is_coasting = 0;
}
#endif
#if ADC_ENABLED
// Trigger the adc sampling hardware. This triggers the position and temperature sample
adc_start(ADC_FIRST_CHANNEL);
#endif
}
// Wait for the samples to complete
#if TEMPERATURE_ENABLED
if (adc_temperature_value_is_ready())
{
// Save temperature value to registers
registers_write_word(REG_TEMPERATURE_HI, REG_TEMPERATURE_LO, (uint16_t)adc_get_temperature_value());
}
#endif
#if CURRENT_ENABLED
if (adc_power_value_is_ready())
{
// Get the new power value.
uint16_t power = adc_get_power_value();
// Update the power value for reporting.
power_update(power);
}
#endif
#if ADC_POSITION_ENABLED
if (adc_position_value_is_ready())
{
int16_t position;
// Get the new position value from the ADC module.
position = (int16_t) adc_get_position_value();
#else
if (position_value_is_ready())
{
int16_t position;
// Get the position value from an external module.
position = (int16_t) get_position_value();
#endif
int16_t pwm;
#if BACKEMF_ENABLED
if (emf_motor_is_coasting == 1)
{
uint8_t pwm_a = registers_read_byte(REG_PWM_DIRA);
uint8_t pwm_b = registers_read_byte(REG_PWM_DIRB);
// Quick and dirty check to see if pwm is active
if (pwm_a || pwm_b)
{
// Get the backemf sample.
backemf_get_sample();
// Turn the motor back on
backemf_restore_motor();
emf_motor_is_coasting = 0;
}
}
#endif
// Call the PID algorithm module to get a new PWM value.
pwm = pid_position_to_pwm(position);
#if ALERT_ENABLED
// Update the alert status registers and do any throttling
alert_check();
#endif
// Allow any alerts to modify the PWM value.
pwm = alert_pwm_throttle(pwm);
#if PWM_STD_ENABLED || PWM_ENH_ENABLED
// Update the servo movement as indicated by the PWM value.
// Sanity checks are performed against the position value.
pwm_update(position, pwm);
#endif
#if STEP_ENABLED
// Update the stepper motor as indicated by the PWM value.
// Sanity checks are performed against the position value.
step_update(position, pwm);
#endif
}
// Was a command recieved?
if (twi_data_in_receive_buffer())
{
// Handle any TWI command.
handle_twi_command();
}
// Update the bank register operations
banks_update_registers();
#if MAIN_MOTION_TEST_ENABLED
// This code is in place for having the servo drive itself between
// two positions to aid in the servo tuning process. This code
// should normally be disabled in config.h.
#if CURVE_MOTION_ENABLED
if (motion_time_left() == 0)
{
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 2000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0300);
motion_append();
registers_write_word(REG_CURVE_DELTA_HI, REG_CURVE_DELTA_LO, 1000);
registers_write_word(REG_CURVE_POSITION_HI, REG_CURVE_POSITION_LO, 0x0100);
motion_append();
}
#else
{
// Get the timer.
uint16_t timer = timer_get();
// Reset the timer if greater than 800.
if (timer > 800) timer_set(0);
// Look for specific events.
if (timer == 0)
{
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0100);
}
else if (timer == 400)
{
registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, 0x0300);
}
}
#endif
#endif
}
return 0;
}
