/* Inits motion library. Basically connects your motor settings
* to motor driver and inits mutex for FreeRTOS. */
void motion_init (void)
{
/* TODO: MUTEX INIT*/
/* Init motors */
motor_init(&l_motors[0]);
motor_init(&l_motors[1]);
/* Set current motor acces stationary (free). */
l_cur_access = MOTION_ACCESS_FREE;
}
int main(int argc, char **argv)
{
/* 模块初始化 */
exc_init(); /* 中断初始化 */
sys_timer_init(); /* 系统时钟初始化 */
light_init(); /* LED灯初始化 */
switch_init(); /* 开关初始化 */
speaker_init(); /* 蜂鸣器初始化 */
motor_init(); /* 电机初始化 */
steer_init(); /* 舵机初始化 */
decoder_init(); /* 编码器初始化 */
serial_initialize((intptr_t)(NULL)); /* 初始化串口 */
//sd_init(&Fatfs); /* 初始化SD卡，并创建文件 */
//sd_create_file(&test_data, test_data_name);
/* 命令注册 */
help_cmd_initialize((intptr_t)(NULL));
light_cmd_initialize((intptr_t)(NULL));
switch_cmd_initialize((intptr_t)(NULL));
speaker_cmd_initialize((intptr_t)(NULL));
motor_cmd_initialize((intptr_t)(NULL));
decoder_cmd_initialize((intptr_t)(NULL));
control_cmd_initialize((intptr_t)(NULL));
//sd_cmd_initialize((intptr_t)(NULL));
printf("\n Welcome to k60 software platform!");
printf("\n Press 'help' to get the help! \n");
//light_open(LIGHT4);
/* ntshell测试 */
task_ntshell((intptr_t)(NULL));
}
/**********************************************************************//**
* Purpose : Constructor. This runs 1 motor.
* @param : motor_index [1..8]
* Return : none
*************************************************************************/
DC_Motor::DC_Motor(byte motor_index) :
ForwardDirection( true ),
Enabled ( false ),
MotorIndex( motor_index-1 )
{
motor_init();
};
//-------------------------------------------------------------------
// Musicplayer_Init
//-------------------------------------------------------------------
void vInitMusicplayer (void)
{
// Initialize Audio interface
EVAL_AUDIO_SetAudioInterface( AUDIO_INTERFACE_I2S );
EVAL_AUDIO_Init( OUTPUT_DEVICE_BOTH, 60, I2S_AudioFreq_48k ); // Volume 80%, 44Khz samplefrequentie
Audio_MAL_Play((uint32_t)AUDIO_SAMPLE, sizeof(AUDIO_SAMPLE) ); // Speel stilte, waarover de toongenerator de toon speelt
EVAL_AUDIO_PauseResume(AUDIO_RESUME); // Start met spelen
// Motor init
motor_init();
// User button init
GPIO_InitTypeDef GPIO_InitStructure;
#if 0
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
#endif
/* Enable the BUTTON Clock */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
/* Configure Button pin as input */
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_Init(GPIOA, &GPIO_InitStructure);
#if 0
/* Connect Button EXTI Line to Button GPIO Pin */
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource0);
/* Configure Button EXTI line */
EXTI_InitStructure.EXTI_Line = EXTI_Line2;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
/* Enable and set Button EXTI Interrupt to the lowest priority */
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
// Motor test
// Dit staat hier omdat een van de andere init's niet werkt zonder evdk
//motor_init();
//setLedBlink();
//{int i; for(i=0;i<1000000;i++);} // Small delay for the motor control board to accept the previous command
//// Motor ייn rondje
//SMC_step(1600,1,1000,1);
}
void main(void) {
char buffer[SCI_BUFSIZ+1] = {0};
// Initialize all nessesary modules
timer_init();
SCIinit();
encoder_init();
motor_init();
msleep(16); LCDinit();
//start_heartbeat(); // Not used, TCNT overflow interrupts causing issues
DDRP |= PTP_PTP0_MASK; // Set DDR for laser GPIO
// Motors off initially
motor_set_speed(MOTOR1C, 0);
motor_set_speed(MOTOR2C, 0);
EnableInterrupts;
LCDclear(); LCDputs("Ready.");
SCIputs("HCS12 ready to go!");
for(EVER) {
SCIdequeue(buffer);
cmdparser(buffer);
memset(buffer, 0, SCI_BUFSIZ+1); // Clear out the command buffer after each command parsed
//LCDclear(); LCDprintf("\r%7d %7d\n%7d %7d", drive_value1, drive_value2, speed_error1, speed_error2);
}
}
void lf_init() {
motor_init();
ir_init();
sum = 0;
previous = 0;
}
int main(void) {
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
// configure the CPU clock (MCLK)
// to run from SMCLK: DCO @ 16MHz and SMCLK = DCO
// If no DCO calibration => stop here by loop
if(CALBC1_16MHZ==0xff)
while(1)
_NOP();
_BIC_SR(GIE);
DCOCTL = 0;
BCSCTL1= CALBC1_16MHZ;
DCOCTL = CALDCO_16MHZ;
clock_init();
uart_init();
motor_init();
protocols_init();
pwm_init();
// ----------
// **action**
// ----------
_BIS_SR(GIE);
FOREVER {
// read input
input_scanner();
// handle any pending state changes in motor
motor_step();
// send status reports to pi
reporter();
}
}
void init() {
/**
* Setup default pin config:
*
* 1-16 : Digital Output
* 17-32: Digital Input
*
* Servos on S1 - S12
*
* Motor control on associated pins
*/
out_ports[0] = &PORTF;
out_ports[1] = &PORTK;
out_ports[2] = &PORTA;
out_ports[3] = &PORTC;
in_ports[0] = &PINF;
in_ports[1] = &PINK;
in_ports[2] = &PINA;
in_ports[3] = &PINC;
dir_ports[0] = &DDRF;
dir_ports[1] = &DDRK;
dir_ports[2] = &DDRA;
dir_ports[3] = &DDRC;
servo_init();
motor_init();
}
void init_all() {
init();
motor_init();
ir_init();
init_lcd();
wait();
}
void brain_run(void)
{
/* initialize components */
puts("[brain] initializing front distance sensor...");
if (srf02_init(&dist_front, CONF_DIST_FRONT_I2C, CONF_DIST_FRONT_ADDR) < 0) {
puts("[failed]");
return;
}
puts("[brain] initializing back distance sensor...");
if (srf02_init(&dist_back, CONF_DIST_BACK_I2C, CONF_DIST_BACK_ADDR) < 0) {
puts("[failed]");
return;
}
puts("[brain] initializing steering servo...");
if (servo_init(&steering, CONF_STEERING_PWM, CONF_STEERING_PWM_CHAN,
CONF_STEERING_MIN, CONF_STEERING_MAX) < 0) {
puts("[failed]");
return;
}
servo_set(&steering, CONF_STEERING_CENTER);
puts("[brain] initializing motor driver...");
if (motor_init(&mot, &mot_params)) {
puts("[failed]");
return;
}
/* go and have fun */
puts("[brain] components ready, all up an running!");
thread_create(stack, sizeof(stack),
CONF_PRIO_COL_DETECT, THREAD_CREATE_STACKTEST,
colllision_detection, NULL, "col detect");
}
void init()
{
ResetReason = MCUSR;
cli();
chip_init (); // Chip initialization
init_leds ();
delay(1000000); // ~ 2 sec
read_cal(); // Read everything including motor stops.
// yes can_init() needs MyInstance to be set already for filtering!
can_init(CAN_250K_BAUD); /* Enables Mob0 for Reception! */
// INIT MYINSTANCE:
config_init();
can_instance_init();
set_rx_callback ( can_file_message );
set_configure_callback ( config_change );
sei();
OS_InitTask();
pot_init();
motor_init ();
can_prep_instance_request( &msg2, 0xBB );
can_send_msg( 0, &msg2 );
}
void r3d2_init(void)
{
NOTICE(0,"Initializing timer");
// initialise timer used to get ticks of sensor detection and motor rotation
timer_init();
NOTICE(0,"timer1 starting");
timer1_start();
// initialise port to power on and get signal of the sensor
NOTICE(0,"Initializing sensor");
init_sensor();
// power on the motor that drive the mirror
NOTICE(0,"Initializing motor");
motor_init();
// first there is no robot detected, neither motor error
robot_detected_value = robot_not_detected;
motor_error_value = no_motor_error;
robot_detected_timout_treshold = eeprom_read_byte(&eep_robot_detected_timout_treshold);
surface_reflection_ratio = eeprom_read_float((float*)&eep_surface_reflection_ratio);
robot_detected_angle_offset = eeprom_read_float((float*)&eep_robot_detected_angle_offset);
motor_rotating_timout_treshold = eeprom_read_byte(&eep_motor_rotating_timout_treshold);
motor_rotating_timout_value = motor_rotating_timout_treshold;
robot_detected_timout_value = 0;
}
/*
* Main Loop
*/
int main()
{
LOG("--------------------------------\r\n");
/* Unmask interrupt for output compare match A on TC0 */
timers_setup_timer(TIMER_COUNTER0, TIMER_MODE_CTC, 1000UL);
TIMSK0 |= (1 << OCIE0A);
lcd_load_custom_character(degree_symbol, CUSTOM_SYMBOL_DEGREE);
cli_init();
motor_init(&g_timers_state);
log_init();
scheduler_init(&g_timers_state, g_tasks, COUNT_OF(g_tasks));
interpolator_init(&g_timers_state);
sei();
log_start();
/* Main Loop: Run Tasks scheduled by scheduler */
while (1) {
int i;
for (i = 0; i < 50; i++) {
serial_check(); /* needs to be called frequently */
}
scheduler_service();
}
}
void cmdBangBang(void)
{
motor_init();
motor_left_stop();
motor_right_stop();
motor_left_duty(20);
motor_right_duty(40);
motor_left_forward();
motor_right_forward();
while(1)
{
// start turning left
motor_left_duty(20);
motor_right_duty(40);
myStringPut("Bang Left");
myNLPut();
while (line_status() == WHITE) // WHITE
{
}
// Hits Black
// Turns right
motor_left_duty(40);
motor_right_duty(20);
myStringPut("Bang Right");
myNLPut();
while (line_status() == BLACK) // BLACK
{
}
}
}
int main(){
//initialisation des variable de base
int cont=1;
char str[100];
char chact;
// alocation de la possition de l'evalbot
evalbot = malloc(sizeof(t_possition));
evalbot->x =0;
evalbot->y = 0;
evalbot->angle =0;
evalbot->state =WAIT;
//init
motor_init();
mess_init();
bump_init();
wheel_init();
//on met l'evalbot imobile
motor_STOP();
while(cont){
//get un message
chact = mess_get();
//fait l'action
actionInt(chact);
// detecte si l'evalbot n' a pas de PB
if(evalbot->state != PB){
//regarde l'ordre
switch(chact){
// envoie une info
case 'i':
sprintf(str,"!POS:x:%4i y:%4i a:%4i",
evalbot->x,
evalbot->y,
evalbot->angle);
mess_setString(str,24);
break;
// met pause
case 'p':
motor_STOP();
break;
}
}
/*
tout les autres comportement sont geré
par les interuptions
*/
}
}
int main(void)
{
motor_init();
//while(1)
// rotazione1();
// Initialize system upon power-up.
serial_init(); // Setup serial baud rate and interrupts
/*while(true) {
serial_write('A');
serial_write('B');
serial_write('C');
}*/
//settings_init(); // Load grbl settings from EEPROM
stepper_init(); // Configure stepper pins and interrupt timers
system_init(); // Configure pinout pins and pin-change interrupt
memset(&sys, 0, sizeof(sys)); // Clear all system variables
sys.abort = true; // Set abort to complete initialization
//sei(); // Enable interrupts
// Check for power-up and set system alarm if homing is enabled to force homing cycle
// by setting Grbl's alarm state. Alarm locks out all g-code commands, including the
// startup scripts, but allows access to settings and internal commands. Only a homing
// cycle '$H' or kill alarm locks '$X' will disable the alarm.
// NOTE: The startup script will run after successful completion of the homing cycle, but
// not after disabling the alarm locks. Prevents motion startup blocks from crashing into
// things uncontrollably. Very bad.
#ifdef HOMING_INIT_LOCK
if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; }
#endif
// Grbl initialization loop upon power-up or a system abort. For the latter, all processes
// will return to this loop to be cleanly re-initialized.
for(;;) {
// TODO: Separate configure task that require interrupts to be disabled, especially upon
// a system abort and ensuring any active interrupts are cleanly reset.
// Reset Grbl primary systems.
serial_reset_read_buffer(); // Clear serial read buffer
//spindle_init();
//coolant_init();
//limits_init();
//probe_init();
st_reset(); // Clear stepper subsystem variables.
// Reset system variables.
sys.abort = false;
sys.execute = 0;
// Start Grbl main loop. Processes program inputs and executes them.
protocol_main_loop();
}
return 0; /* Never reached */
}
void init() {
timer_init();
motor_init();
uart_init();
queue_init();
ultrasonic_init();
sei();
}
void all_init() {
init();
ir_init();
init_lcd();
motor_init();
wait();
}
int main(void)
{
_delay_ms(100);
LCD_Initalize();
LCD_WriteText("Anal Intruder 1");
LCD_GoTo(0, 1);
LCD_WriteText("8=======D (.)(.)");
pad_init();
motor_init();
brzeczyk_init();
sensors_init();
sei();
play(power, 11);
while(1)
{
while(!pad_get_state()) // sprawdzenie po³¹czenia z padem
{
m1_stop();
m2_stop();
LCD_Clear();
LCD_WriteText("Pad conn error!");
LCD_GoTo(0, 1);
LCD_WriteText("Reconnecting...");
_delay_ms(500);
pad_init();
tryb = 0;
lcd_old = -1;
motor_lcd = false;
}
lcd_tryb();
switch(tryb)
{
case 0:
if(!(tab[4] & (1 << 4))) // jeœli wciœniêty trojkat
tryb = 1;
else if(!(tab[4] & (1 << 5))) // jeœli wciœniête jest kolko
tryb = 2;
break;
case 1:
pad_loop();
break;
case 2:
sensors_loop();
break;
}
if(!(tab[3] & (1 << 3)))
{ // jeœli wciœniêty start
tryb = 0;
m1_stop();
m2_stop();
srednia = 0;
}
}
}
//MOTOR SPEED NEEDS TO BE TUNED FOR INDIVIDUAL GAME BOARD
void motor_controller_calibrate_by_reset(){
uint8_t speed = 65;
motor_init();
uint16_t position;
uint16_t prev_position;
motor_speed(speed);
motor_direction(right);
_delay_ms(150);
position = motor_encoder_read();
//go right until stopped, then set encoder to zero
while(position != prev_position){
position = motor_encoder_read();
_delay_ms(100);
prev_position = position;
position = motor_encoder_read();
printf("position: %u\tPrev: %u\n", position, prev_position);
}
motor_encoder_reset();
motor_speed(speed);
motor_direction(left);
_delay_ms(150);
//go left until stopped, set max left to current position
do{
position = motor_encoder_read();
_delay_ms(100);
prev_position = position;
position = motor_encoder_read();
printf("position: %d\n", position);
} while(position != prev_position);
max_left = position;
motor_speed(0);
motor_speed(speed);
motor_direction(right);
_delay_ms(100);
while(motor_encoder_read() > 4500){
_delay_ms(100);
}
}
int main(void)
{
struct heading result;
/*u08 k=0;
u08 b=0;
u08 choice=0;
u08 button=0;*/
u08 ir=0;
u08 i=0;
x = 64;
y = 48;
initialize();
motor_init();
servo_init();
set_motor_power(0,0);
set_motor_power(1,0);
set_motor_power(2,0);
set_motor_power(3,0);
compass_init();
sonar_init();
calCompass();
while(1)
{
clear_screen();
result = compass();
/* use calibration data */
result.x -= compZero.x;
result.y -= compZero.y;
print_string("x "); /* 2 */
print_int(result.x); /* 3 */
print_string(" y "); /* 3 */
print_int(result.y); /* 3 */
/* print_string(" s "); */ /* 3 */
/* print_int(getSonar(0)); */ /* 3 */
/* =17 */
next_line();
/* print_string("a "); */ /* 2 */
/* print_int(IR(0)); */ /* 3 */
/* print_string(" b "); */ /* 3 */
/* print_int(analog(1)); */ /* 3 */
/* print_string(" c "); */ /* 3 */
/* print_int(analog(2)); */ /* 3 */
/* print_int(i++); */ /* =17 */
/* print_int(distance(0));
print_string(" "); */
print_int(sizeof(int));
delay_ms(200);
/* print_int(i);
i=sweep(); */
}
}
int main(void)
{
lb_init(&lb);
event_init();
motor_init();
uart_init(); // init USART
enc_init();
i2c_init();
adc_init();
kalman_init();
sei(); // enable interrupts
// Wait a second at startup
_delay_ms(1000);
// send initial string
printf_P(PSTR("Hello world!\n"));
imu_init();
for (;/*ever*/;)
{
// ADCSRA |= (1<<ADSC); // Set start conversion bit and wait for conversion to finish
// while(ADCSRA&(1<<ADSC));
// OCR1AL = ADCH; // Set ADC reading to timer 0 compare
if(event_pending())
{
event_action();
}
else // No pending operation, do low priority tasks
{
// dequeue receive buffer if any bytes waiting
while (uart_avail())
{
char c = uart_getc();
if (lb_append(&lb, c) == LB_BUFFER_FULL)
{
lb_init(&lb); // Clear line
printf_P(PSTR("\nMax line length exceeded\n"));
}
// Process command if line buffer is ready ...
if (lb_line_ready(&lb))
{
strcpy(cmd_string,lb_gets(&lb));
do_cmd(cmd_string);
lb_init(&lb);
}
}
}
// Process command if line buffer is terminated by a line feed or carriage return
}
return 0;
}
void ICACHE_FLASH_ATTR
motor_object_init() {
motor_init();
pando_object motor_object = {
1,
motor_object_pack,
motor_object_unpack,
};
register_pando_object(motor_object);
}
static inline void init(void)
{
power_all_disable();
debug_led_init();
clock_prescale_set(clock_div_1);
spi_io_init();
adc_init();
motor_init();
sei();
}
void init(void)
{
hardware_lowlevel_init();
rtc_init_flag();
spi_drv_init();
spi_ext_init();
drv8301_init();
pid_init();
commutate_init();
motor_init();
EnableInterrupts; // enable Interrupts
}
void
motor_module_init()
{
motor_init();
struct module_stru temp;
temp.module_head = MOD_MOTOR_HEAD;
temp.count_tasks = 1;
temp.dispatch = motor_dispatch;
temp.taks_init[0] = motor_task_init;
module_addtolist(temp, MOD_MOTOR_HEAD);
}
void init(){
led_init();
motor_init();
switch_init();
ground_init();
dist_init();
servo_init();
if(DEBUG) usart_init();
motor1 = Motor(&OCR1A, &MOTOR1_DIR_PORT, MOTOR1_DIR_PIN, MAX_POWER);
motor2 = Motor(&OCR1B, &MOTOR2_DIR_PORT, MOTOR2_DIR_PIN, MAX_POWER);
}
void wheel_init (void)
{
pid_enable = TRUE; /* default enable PID */
encoder_init(); /* initialize tacho encoders */
motor_init(); /* initialize PWM */
motor_set_param (WHEEL_LEFT, pid_interval*10, 1, 0, 0); /* dT=100ms, Kp=1 Ki=0, Kd=0 */
pid_int_init (&pid_l); /* initialize PID controller (left) */
motor_set_param (WHEEL_RIGHT, pid_interval*10, 1, 0, 0); /* dT=100ms, Kp=1 Ki=0, Kd=0 */
pid_int_init (&pid_r); /* initialize PID controller (right) */
}
void init(void){
NUMBER_OF_PACKETS = 0;
TIMER_OVERFLOW = 0;
uart_init(UART_BAUD_SELECT(MASTER_UART_BAUD_RATE, F_CPU ));
//All initiialization is to be done here.
softuart_init(PORT_1);
softuart_init(PORT_2);
adc_init();
sei();
motor_init(); // enable motors after both soft uart ports are enabled.
timer_init();
}
int main(void)
{
DDRC |= 1<<LED2;
DDRC |= 1<<LED3;
init_TWI();
motor_init();
sei();
while(1)
{
if(new_cmd_flag){
process_cmd(cmd_buf);
}
}
}
