/* Turn off all motion */ void stop_motion(void) { // Turn off motors motor_set_speed(MOTOR1C, 0); motor_set_speed(MOTOR2C, 0); msleep(20); // Wait for motors to come to a stop TC_INT_DISABLE(TC_MOTOR); // Disable motor control law }
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 motor_set_speed_all(uint16_t speed) { motor_set_speed(MOTOR_1, speed); motor_set_speed(MOTOR_2, speed); motor_set_speed(MOTOR_3, speed); motor_set_speed(MOTOR_4, speed); }
void motor_set_cmd(uint8_t chan, uint8_t mode, uint8_t speed) { uint16_t s = ((float)speed / 255) * 1000; if (mode == 2) { // fw motor_set_dir(chan, MOTOR_FW); motor_set_speed(MOTOR_1, s); } else if (mode == 1) { // bw motor_set_dir(chan, MOTOR_BW); motor_set_speed(MOTOR_1, s); } else if (mode == 3) { // brake motor_set_dir(chan, MOTOR_BRAKE); motor_stop(MOTOR_1); } }
void motor_set(motor_chan_t motor, int16_t speed) { if (speed < 0) { speed = speed * -1; motor_set_dir(motor, MOTOR_FW); } else { motor_set_dir(motor, MOTOR_BW); } speed = (speed > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed; motor_set_speed(motor, speed); }
void emergency_stop(char* errMsgLine1, char* errMsgLine2) { motor_set_speed(NXT_PORT_A,0,1); motor_set_speed(NXT_PORT_B,0,1); motor_set_speed(NXT_PORT_C,0,1); print_clear_display(); print_str(0,1,errMsgLine1); print_str(0,2,errMsgLine2); int i; for(i = 0; i < 3; i++) { print_str(6,4,"ERROR"); print_update(); ecrobot_set_light_sensor_active(NXT_PORT_S1); ecrobot_set_light_sensor_active(NXT_PORT_S2); ecrobot_set_light_sensor_active(NXT_PORT_S3); ecrobot_sound_tone(1000,500,15); systick_wait_ms(750); print_clear_line(4); print_update(); ecrobot_set_light_sensor_inactive(NXT_PORT_S1); ecrobot_set_light_sensor_inactive(NXT_PORT_S2); ecrobot_set_light_sensor_inactive(NXT_PORT_S3); ecrobot_sound_tone(500,500,15); systick_wait_ms(750); } motor_set_speed(NXT_PORT_A,0,0); motor_set_speed(NXT_PORT_B,0,0); motor_set_speed(NXT_PORT_C,0,0); while(true) { print_str(6,4,"ERROR"); print_update(); ecrobot_set_light_sensor_active(NXT_PORT_S1); ecrobot_set_light_sensor_active(NXT_PORT_S2); ecrobot_set_light_sensor_active(NXT_PORT_S3); systick_wait_ms(750); print_clear_line(4); print_update(); ecrobot_set_light_sensor_inactive(NXT_PORT_S1); ecrobot_set_light_sensor_inactive(NXT_PORT_S2); ecrobot_set_light_sensor_inactive(NXT_PORT_S3); systick_wait_ms(750); } }
/***********************cmdparser******************************* * * Purpose: Parse the command string to call the correct function. * * Input: char *cmdtype: input command string. * * Output: int result: Resulting integer value. * ***************************************************************/ void cmdparser(char *buffer) { char cmdtype[CMD_LEN+1] = {0}; int numchars = 0; static int numcmd = 0; // Count of number of commands parsed static byte tog = 0; // Laser toggle bit char motor1_pct, motor2_pct; cmdtype[0] = buffer[numchars]; cmdtype[1] = buffer[numchars+1]; cmdtype[2] = buffer[numchars+2]; cmdtype[CMD_LEN] = '\0'; // Terminate input command after three bytes, leaving just the command type switch(cmdconv(cmdtype)) { case 0: // If no command found, go to next character. seekcmd(buffer, &numchars); break; case PNG: // ping SCIprintf("png%05d",numcmd); // echo command confirmation with stamp. //LCDclear(); LCDputs("Ping!"); numcmd++; numchars += SCI_CMDSIZ; break; case ABT: // STOP THE PRESS! SCIprintf("abt%05d",numcmd); LCDclear(); LCDputs("Abort!\nAbort!"); stop_motion(); numcmd++; numchars += SCI_CMDSIZ; break; case RES: // Resume operation SCIprintf("res%05d",numcmd); LCDclear(); LCDputs("Resuming..."); start_motion(); LCDclear(); LCDputs("Resumed"); numcmd++; numchars += SCI_CMDSIZ; break; case MOV: // Set motor speed (0% - 100%) SCIprintf("mov%05d", numcmd); if(buffer[numchars+3] == '2') { // Both motors selected TC_INT_DISABLE(TC_MOTOR); // Disable motor control law motor_set_speed(MOTOR1C, (char)atoi(&buffer[numchars+4])); motor_set_speed(MOTOR2C, (char)atoi(&buffer[numchars+4])); TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law //LCDclear(); LCDprintf("\rM%c: %3d M%c: %3d", MOTOR1C, atoi(&buffer[numchars+4]), MOTOR2C, atoi(&buffer[numchars+4])); } else { motor_set_speed(buffer[numchars+3], (char)atoi(&buffer[numchars+4])); //LCDclear(); LCDprintf("\rMotor %c: %3d", buffer[numchars+3], atoi(&buffer[numchars+4])); } numcmd++; numchars += SCI_CMDSIZ; break; case DST: // Set motor distance (+speed) SCIprintf("dst%05d", numcmd); switch(buffer[numchars+4]) { case '0': // Setting a speed motor1_pct = motor_convert(MOTOR1C, (int)atoi(&buffer[numchars+5])); motor2_pct = motor_convert(MOTOR2C, (int)atoi(&buffer[numchars+5])); // Set speed to both motors if 4th char is a '2' if(buffer[numchars+3] == '2') { TC_INT_DISABLE(TC_MOTOR); // Disable motor control law motor_set_speed(MOTOR1C, motor1_pct); motor_set_speed(MOTOR2C, motor2_pct); TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law } else motor_set_speed(buffer[numchars+3], motor_convert(buffer[numchars+3], (int)atoi(&buffer[numchars+5])) ); //LCDclear(); LCDprintf("\rM1: %3d M2: %3d", motor1_pct, motor2_pct); //LCDprintf("\nS1: %3d S2: %3d", atoi(&buffer[numchars+5]), atoi(&buffer[numchars+5])); break; case '1': // Setting a distance // Set speed to both motors if 4th char is a '2' if(buffer[numchars+3] == '2') { motor_set_distance(MOTOR1C, (word)atoi(&buffer[numchars+5])); motor_set_distance(MOTOR2C, (word)atoi(&buffer[numchars+5])); //LCDclear(); LCDprintf("\nD%c: %3d D%c: %3d", MOTOR1C, atoi(&buffer[numchars+5]), MOTOR2C, atoi(&buffer[numchars+5])); } else { motor_set_distance(buffer[numchars+3], (word)atoi(&buffer[numchars+5])); //LCDclear(); LCDprintf("\rDist %c: %3d", buffer[numchars+3], atoi(&buffer[numchars+5])); } break; } numcmd++; numchars += SCI_CMDSIZ; break; case SPN: // Spin in place SCIprintf("spn%05d", numcmd); DisableInterrupts; motor_set_speed(MOTOR1C, -50); motor_set_speed(MOTOR2C, 50); motor_set_distance(MOTOR1C, (word)atoi(&buffer[numchars+3])); motor_set_distance(MOTOR2C, (word)atoi(&buffer[numchars+3])); EnableInterrupts; SCIprintf("Dist: %3d\n", atoi(&buffer[numchars+3])); numcmd++; numchars += SCI_CMDSIZ; break; case AIM: // Toggle laser pointer SCIprintf("aim%05d",numcmd); tog = (tog) ? 0 : 1; PTP_PTP0 = (tog) ? 1 : 0; numcmd++; numchars += SCI_CMDSIZ; break; case STP: // Force stop; set motor PWM to zero to stop the high frequency ringing! SCIprintf("stp%05d",numcmd); TC_INT_DISABLE(TC_MOTOR); // Disable motor control law motor_set_duty(MOTOR1C, 0); motor_set_duty(MOTOR2C, 0); TC_INT_ENABLE(TC_MOTOR); // Re-enable motor control law numcmd++; numchars += SCI_CMDSIZ; break; } }
/** * Function to process debugging module * Checks for debbunging commands and things like this */ void debug_process(void){ #ifdef CFG_SUART_RX // check for recieved commands if( suart_rxFlag ){ uint8_t cmd = suart_getc(); #if !defined(CFG_CODE_LEVEL_AVG) uint16_t tmp; #endif // process command switch(cmd){ case 'f': motor_set_speed(MOTOR_ADDR_L, 0x50, MOTOR_FORWARD); motor_set_speed(MOTOR_ADDR_R, 0x40, MOTOR_FORWARD); break; case'b': motor_set_speed(MOTOR_ADDR_L, 0x40, MOTOR_BACKWARD); motor_set_speed(MOTOR_ADDR_R, 0x43, MOTOR_BACKWARD); break; case 'z': motor_set_speed(MOTOR_ADDR_L, 0x00, MOTOR_BRAKE); motor_set_speed(MOTOR_ADDR_R, 0x00, MOTOR_BRAKE); break; #if !defined(CFG_CODE_LEVEL_AVG) case 'g': led_set_allcolors(); led_all_on(); break; case 'h': led_set_nocolors(); led_all_off(); break; #endif case 's': led_on(LED_STATUS); break; case 'r': led_off(LED_STATUS); break; case 'c': motor_set_speed(MOTOR_ADDR_L, MOTOR_SPEED_HALF, MOTOR_FORWARD); motor_set_speed(MOTOR_ADDR_R, MOTOR_SPEED_HALF, MOTOR_BACKWARD); break; case 'a': motor_set_speed(MOTOR_ADDR_L, MOTOR_SPEED_HALF, MOTOR_BACKWARD); motor_set_speed(MOTOR_ADDR_R, MOTOR_SPEED_HALF, MOTOR_FORWARD); break; #if !defined(CFG_CODE_LEVEL_AVG) case 'm': debug_send_c( motor_get_speed(MOTOR_ADDR_L), TRUE ); break; case 'n': debug_send_c( motor_get_speed(MOTOR_ADDR_R), TRUE ); break; case 'o': debug_send_c( motor_get_direction(MOTOR_ADDR_L), TRUE ); break; case 'p': debug_send_c( motor_get_direction(MOTOR_ADDR_R), TRUE ); break; case 'd': debug_send_c( motor_get_fault(MOTOR_ADDR_L), TRUE ); break; case 'e': debug_send_c( motor_get_fault(MOTOR_ADDR_R), TRUE ); break; case 'u': motor_clear_fault( MOTOR_ADDR_L ); break; case 'v': motor_clear_fault( MOTOR_ADDR_R ); break; #endif case 'i': debug_send_c( sys_robotID, TRUE ); break; case 'j': debug_send_msg( SYS_PUBLISHER, TRUE ); break; case 'k': debug_send_msg( SYS_VERSION, TRUE ); break; case 'l': debug_send_msg( SYS_NAME, TRUE ); break; case 'M': debug_send_c( motor_test(), TRUE ); break; #if !defined(CFG_CODE_LEVEL_AVG) case 'T': tmp = monitor_read_temp(MONITOR_ADDR_1); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, FALSE ); tmp = monitor_read_temp(MONITOR_ADDR_2); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'U': tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U1); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'V': tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U4); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'W': tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U2); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'X': tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U3); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'I': tmp = monitor_read_current(MONITOR_ADDR_1, MONITOR_I1); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'J': tmp = monitor_read_current(MONITOR_ADDR_1, MONITOR_I2); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'A': tmp = monitor_read_voltage(MONITOR_ADDR_1, MONITOR_UVCC); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, FALSE ); tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_UVCC); debug_send_c_wait( tmp>>8, FALSE ); debug_send_c_wait( tmp, TRUE ); break; case 'R': animation_set_mode(ANIMATION_MODE_RED_PULSED); break; case 'S': animation_set_mode(ANIMATION_MODE_STROBE); break; case 'F': animation_set_mode(ANIMATION_MODE_FADE); break; case 'L': animation_set_mode(ANIMATION_MODE_LADY); break; case 'N': animation_set_mode(ANIMATION_MODE_NONE); break; #endif case '?': debug_send_help(); break; default: debug_send_c( 0xff, TRUE ); break; } } #endif /* CFG_SUART_RX */ }
void kill_motors(void) { motor_set_speed('r', 0); motor_set_speed('l', 0); }
int main(void) { SystemInit(); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); // STM32F4_Discovery_LEDInit(LED3); //Orange STM32F4_Discovery_LEDInit(LED4); //Green STM32F4_Discovery_LEDInit(LED5); //Red STM32F4_Discovery_LEDInit(LED6); //Blue STM32F4_Discovery_PBInit(BUTTON_USER, BUTTON_MODE_GPIO); USBD_Init(&USB_OTG_dev,USB_OTG_FS_CORE_ID,&USR_desc,&USBD_CDC_cb,&USR_cb); led_driver_init(); motor_driver_init(); SysTick_Config(2100000); //10 times a second: 168 MHz / 8 = 21 MHz SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8); //while(1) //{ //led_driver_write_data(50, 10); //} while (1) { if( usb_cdc_kbhit() ) { character = usb_cdc_getc(); usb_rx_buffer[usb_rx_ptr] = character; usb_rx_ptr++; if( usb_rx_ptr == 10 ) //upon receiving 10 bytes { if( ( usb_rx_buffer[0] == 0xAA ) && ( usb_rx_buffer[9] == 0xCC ) ) //check head and tail for correctness { usb_rx_ptr = 0; //reset data pointer if correct flag_new_command_received = 1; //set the flag if packet correct } else { usb_rx_ptr = 0; // if packet incorrect, reset data pointer } } } if(flag_new_command_received) { flag_new_command_received = 0; } if(flag_send_sensor_data) { motor_set_speed(0x55, 0x55); flag_send_sensor_data = 0; } // here's how to send data through USB VCP /* sprintf(buffer_out,"LED%c = %u\r\n",c,GPIO_ReadInputDataBit(GPIOD,LED6_PIN)); usb_cdc_printf(buffer_out); */ } }
int main(void){ uint16_t curMotorL = 0x00; uint16_t curMotorR = 0x00; uint8_t speedMotorL = 0x00; uint8_t speedMotorR = 0x00; // initiate system sys_init(); // main loop while(1){ // suspend system sys_sleep(); // check for changed motor values if( ctrl_flag_motorL ){ motor_set_speed( MOTOR_ADDR_L, control_getMotorSpeed(MOTOR_ADDR_L), control_getMotorCommand(MOTOR_ADDR_L)); } if( ctrl_flag_motorR ){ motor_set_speed( MOTOR_ADDR_R, control_getMotorSpeed(MOTOR_ADDR_R), control_getMotorCommand(MOTOR_ADDR_R)); } // check for changed robot id if( ctrl_flag_id ){ sys_ee_set_robotID( control_getRobotID() ); #ifdef CFG_EXTERNAL_MARKER marker_set_id(sys_robotID); #endif } // read motor currents and speeds #if !defined(CFG_CODE_LEVEL_MIN) curMotorL = monitor_read_current(MONITOR_ADDR_1, MONITOR_I1); curMotorR = monitor_read_current(MONITOR_ADDR_1, MONITOR_I2); #endif speedMotorL = control_getMotorSpeed(MOTOR_ADDR_L); speedMotorR = control_getMotorSpeed(MOTOR_ADDR_R); // adjust motor speed // tries to decrease speed of faster motor, // or if that fails because motor is already speed minimum // increases the speed of the slower motor. // if( curMotorL > curMotorR ){ // // if( speedMotorL > MOTOR_SPEED_MIN ){ // motor_decrease_speed(MOTOR_ADDR_L, 1); // } else if( speedMotorR < MOTOR_SPEED_MAX ){ // motor_increase_speed(MOTOR_ADDR_R, 1); // } // // } else if( curMotorR > curMotorL ){ // // if( speedMotorR > MOTOR_SPEED_MIN ){ // motor_decrease_speed(MOTOR_ADDR_R, 1); // } else if( speedMotorL < MOTOR_SPEED_MAX ){ // motor_increase_speed(MOTOR_ADDR_L, 1); // } // // } #ifndef CFG_CODE_LEVEL_MIN // process debugging module debug_process(); #endif } }
void motor_full_bw(motor_chan_t motor) { motor_set_dir(motor, MOTOR_BW); motor_set_speed(motor, MOTOR_SPEED_MAX); }
void motor_stop(motor_chan_t motor) { motor_set_speed(motor, 0); }