/* 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);
}
