int main(void) {
di();
OSCCON = 0x78;
led_init();
//sound_init();
radio_init();
motors_init();
adc_init();
ei();
while (1) {
/* Listen for incoming radio packet */
if (radio_rx()) {
/* If a radio packet is pending
* send it immediately
*/
if (radio_len != 0) {
/* Send a packet */
unsigned char c;
LATB |= 0x01;
radio_tx_start();
radio_tx_data(radio_len);
for (c = 0; c < radio_len; c++)
radio_tx_data(radio_data[c]);
radio_tx_finish();
LATB &= ~0x01;
}
}
/* Read ADC values sequentally */
{
static char adc_ptr = 0;
const char adc_addr[6] = {4, 15, 11, 9, 30, 13};
/* Reading value */
char adc_id = adc_addr[adc_ptr];
adc_measure(adc_id);
adc_data[adc_ptr << 1] = ADRESH;
adc_data[(adc_ptr << 1) | 1] = ADRESL;
/* Incrementing address */
adc_ptr++;
if (adc_ptr >= 6)
adc_ptr = 0;
#ifdef FALSE_DEF
/* Checking battery voltage */
if (adc_id == 13) {
/* Voltage less then 3.0V */
if (ADRESH == 0 && ADRESL < 144) {
/* Power off */
LATA = 0;
LATB = 0;
LATC = 0;
di();
SLEEP();
}
}
#endif
}
}
}
int main()
{
motors_init();
movman_init();
contacts_init();
event_q_init();
adc_init();
leds_init();
sei();
adc_start();
movman_schedule_move(
WAIT_5_SECONDS_THEN_FULL_FORWARD_FOR_A_LONG_TIME,
TO_MEET_STARTUP_REQUIREMENT,
IMMEDIATELY);
while(1){
// Testing in the lab showed this runs every ~95us
event_t e = event_q_get_next_event();
switch(e){
case LINE_DETECTED:
handle_line_detected();
break;
case CONTACT_DETECTED_BOTH:
case CONTACT_DETECTED_FRONT:
handle_front_contact();
break;
case CONTACT_DETECTED_REAR:
handle_rear_contact();
break;
case MOVEMENT_COMPLETE:
handle_movement_complete();
break;
case NEW_PROXIMITY_READINGS:
handle_new_prox_readings();
break;
default:
break;
}
}
return 0;
}
int main(void)
{
/*init all the things*/
motors_init(PHASE_CORRECT, PRESCALE_8);
uart_init(BAUD_CALC(9600));
sei();
adc_init();
wdt_init(WDTO_500MS);
/********************/
go(0, 0);
uart_putc(ACK);
over_current_a = 0;
over_current_b = 0;
while (1)
{
current_a = analog_read(C_SENS_A);
current_b = analog_read(C_SENS_B);
req_t = get_request_type();
if (req_t == CURRENT_REQ)
{
write_current(current_a, current_b);
}
else if (req_t == MOTORS_SET)
{
read_motors(&speed_a, &speed_b);
wdt_reset();
}
//manage current sens
#ifdef CURRENT_LIMIT
if ((current_a < CURRENT_LIMIT) && (current_b < CURRENT_LIMIT))
{
go(speed_a, speed_b);
over_current_a = over_current_b = 0;
}
else
{
//overcurrent
go(0, 0);
uart_putc('!');
over_current_a = current_a - CURRENT_LIMIT;
over_current_b = current_b - CURRENT_LIMIT;
}
#endif
}
return (0);
}
void main(void)
//-----------------------------------------------------------------------------------------------------
// Purpose: The MCU will come here after reset.
//
//
// Rev: 1.5a TEMPLATE RELEASE
//
// Notes: None
//-----------------------------------------------------------------------------------------------------
{
//Initialization function calls
init_ports();
motors_init();
system_clock_init();
InitDisplay("FHBTEST "); //Start-up splash changed to unity ID
InitUART();
ADInit();
ENABLE_SWITCHES;
/* LED initialization - macro defined in qsk_bsp.h */
ENABLE_LEDS
//Polling for switch presses
while(TRUE)
{
if(S1 == PRESSED)
{
BNSPrintf(LCD,"\tTEST \n ");
project2ADemo();
}
else if (S2 == PRESSED)
{
BNSPrintf(LCD,"\tOff \n ");
}
else if (S3 == PRESSED)
{
BNSPrintf(LCD,"\tFigure 8 \n ");
DoFigureEight();
}
else
{
BNSPrintf(LCD,"\tTeam 2 \n ");
}
}
}
void fc_init() {
pids_init();
motors_init();
}
void navigation_update_state()
{
int32_t distance_values[N_ULTRASONIC_SENSORS];
int32_t smallest = 0;
int32_t left_s = 0;
int32_t right_s = 0;
int32_t front1_s = 0;
int32_t front2_s = 0;
ultrasonic_get_distance(distance_values);
smallest = ultrasonic_get_smallest(distance_values, N_ULTRASONIC_SENSORS);
left_s = distance_values[US_LEFT];
right_s = distance_values[US_RIGHT];
front1_s = distance_values[US_FRONT_1];
front2_s = distance_values[US_FRONT_2];
if(navigation_status.current_state == BYPASS){
//motor command is done directly in CLI
}
else if(navigation_status.current_state == AVOID_OBSTACLE){
if (smallest > navigation_status.max_dist_obs){
navigation_status.current_state = GO_TO_TARGET;
GPIO_write(Board_OBS_A_EN, 0);
}else if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)){
navigation_status.current_state = SPACE_NEEDED;
}else if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD){
serial_printf(cli_stdout, "Wall ahead!\n");
navigation_status.current_state = AVOID_WALL;
}
}
else if (navigation_status.current_state == AVOID_WALL){
//We check if we're not facing the wall anymore
if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)){
navigation_status.current_state = SPACE_NEEDED;
}
if (!((front1_s < BACKWARD_THRESHOLD) && (front2_s < BACKWARD_THRESHOLD))){
if (smallest > navigation_status.max_dist_obs){
navigation_status.current_state = GO_TO_TARGET;
GPIO_write(Board_OBS_A_EN, 0);
}else{
navigation_status.current_state = AVOID_OBSTACLE;
}
}
}
else if (navigation_status.current_state == SPACE_NEEDED){
if (!((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2))){
//Check if we face another type of obstacle or if path is free
if (smallest < navigation_status.max_dist_obs){
//We use a different strategy depending on the type of obstacle
if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD){
serial_printf(cli_stdout, "Wall ahead!\n");
navigation_status.current_state = AVOID_WALL;
}else
navigation_status.current_state = AVOID_OBSTACLE;
}else{
navigation_status.current_state = GO_TO_TARGET;
GPIO_write(Board_OBS_A_EN, 0);
}
}
}
else
{
//normal operation: continue on mission items
if(mission_items.item[mission_items.current_index].current)
{
// Only go to target if mb is armed and rover must go to home OR sbc is armed and ready to record
#ifdef CONTINUE_WAYPOINTS_IMMEDIATELY
if(comm_mainboard_armed() == 1){
#else
if(comm_mainboard_armed() == 1 && (mission_items.current_index == 0 || comm_sbc_armed() == 1)){
#endif
navigation_status.current_state = GO_TO_TARGET;
}
else
{
navigation_status.current_state = STOP;
}
}
if(navigation_status.current_state == GO_TO_TARGET)
{
ultrasonic_get_distance(distance_values);
if(navigation_status.distance_to_target < TARGET_REACHED_DISTANCE)
{
navigation_mission_item_reached();
}
else if (smallest < navigation_status.max_dist_obs)
{
GPIO_write(Board_OBS_A_EN, 1);
//We use a different strategy depending on the type of obstacle
if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD)
{
serial_printf(cli_stdout, "Wall ahead!\n");
navigation_status.current_state = AVOID_WALL;
}
else if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2))
{
navigation_status.current_state = SPACE_NEEDED;
}
else
navigation_status.current_state = AVOID_OBSTACLE;
}
}
}
}
#if defined (NAVIGATION_TEST)
void navigation_move();
#else
void navigation_move()
{
static int32_t lspeed, rspeed;
double angular = 0;
//only move if not on halt AND state = go_to_target
if(navigation_status.halt == 0)
{
if(navigation_status.current_state == GO_TO_TARGET)
{
angular = pid_update(&pid_a, navigation_status.angle_to_target) * PID_SCALING_FACTOR;
if (angular > 0){
lspeed = PWM_SPEED_100;
rspeed = PWM_SPEED_100 - (int32_t)(angular);
if (rspeed < PWM_MIN_CURVE_SPEED)
rspeed = PWM_MIN_CURVE_SPEED;
}else if (angular <= 0){
rspeed = PWM_SPEED_100;
lspeed = PWM_SPEED_100 + (int32_t)(angular);
if (lspeed < PWM_MIN_CURVE_SPEED)
lspeed = PWM_MIN_CURVE_SPEED;
}
motors_wheels_move((int32_t)lspeed, (int32_t)rspeed, (int32_t)lspeed, (int32_t)rspeed);
navigation_status.motor_values[0] = lspeed; //backup
navigation_status.motor_values[1] = rspeed;
}
else if(navigation_status.current_state == STOP)
{
motors_wheels_stop();
navigation_status.motor_values[0] = 0;
navigation_status.motor_values[1] = 0;
}
// else if(navigation_status.current_state == AVOID_OBSTACLE)
// {
// int32_t distance_values[N_ULTRASONIC_SENSORS];
// ultrasonic_get_distance(distance_values);
// ultrasonic_check_distance(distance_values, navigation_status.motor_values, PWM_SPEED_80); //80% of speed to move slower than in normal mode
//
// motors_wheels_move(navigation_status.motor_values[0], navigation_status.motor_values[1],
// navigation_status.motor_values[0], navigation_status.motor_values[1]);
// }else if (navigation_status.current_state == AVOID_WALL){
// // move backwards in opposite curving direction (TODO: to be tested)
// lspeed = -navigation_status.motor_values[1];
// rspeed = -navigation_status.motor_values[0];
// motors_wheels_move((int32_t)lspeed, (int32_t)rspeed, (int32_t)lspeed, (int32_t)rspeed);
// }else if (navigation_status.current_state == SPACE_NEEDED){
// navigation_status.motor_values[0] = -PWM_SPEED_100;
// navigation_status.motor_values[1] = -PWM_SPEED_100;
// motors_wheels_move(navigation_status.motor_values[0], navigation_status.motor_values[1],
// navigation_status.motor_values[0], navigation_status.motor_values[1]);
// }
}
else
{
navigation_status.motor_values[0] = 0;
navigation_status.motor_values[1] = 0;
motors_wheels_stop();
}
// serial_printf(cli_stdout, "speed l=%d, r=%d \n", motor_values[0], motor_values[1]);
}
#endif
void navigation_change_gain(char pid, char type, float gain)
{
if (pid == 'a'){
if(type == 'p')
pid_a.pGain = gain;
else if (type == 'i')
pid_a.iGain = gain;
else if (type == 'd')
pid_a.dGain = gain;
}
}
#if defined (NAVIGATION_TEST)
void navigation_init();
#else
void navigation_init()
{
cli_init();
motors_init();
ultrasonic_init();
pid_init(&pid_a, PGAIN_A, IGAIN_A, DGAIN_A, MOTOR_IMAX, MOTOR_IMIN);
navigation_status.lat_rover = INIT_LAT;
navigation_status.lon_rover = INIT_LON;
navigation_status.heading_rover = 0.0;
navigation_status.lat_target = TARGET_LAT;
navigation_status.lon_target = TARGET_LON;
navigation_status.distance_to_target = 0.0;
navigation_status.angle_to_target = 0.0;
navigation_status.current_state = STOP;
navigation_status.max_dist_obs = OBSTACLE_MAX_DIST;
navigation_status.halt = 0;
navigation_status.position_valid = false;
GPIO_write(Board_OBS_A_EN, 1);
mission_items.count = 0;
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
syslog(LOG_INFO, "initializing core");
/* init SCL subsystem: */
syslog(LOG_INFO, "initializing signaling and communication link (SCL)");
if (scl_init("core") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
/* init params subsystem: */
syslog(LOG_INFO, "initializing opcd interface");
opcd_params_init("core.", 1);
/* initialize logger: */
syslog(LOG_INFO, "opening logger");
if (logger_open() != 0)
{
syslog(LOG_CRIT, "could not open logger");
exit(EXIT_FAILURE);
}
syslog(LOG_CRIT, "logger opened");
sleep(1); /* give scl some time to establish
a link between publisher and subscriber */
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "| core startup |");
LOG(LL_INFO, "+------------------+");
LOG(LL_INFO, "initializing system");
/* set-up real-time scheduling: */
struct sched_param sp;
sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
sched_setscheduler(getpid(), SCHED_FIFO, &sp);
if (mlockall(MCL_CURRENT | MCL_FUTURE))
{
LOG(LL_ERROR, "mlockall() failed");
exit(EXIT_FAILURE);
}
/* initialize hardware/drivers: */
omap_i2c_bus_init();
baro_altimeter_init();
ultra_altimeter_init();
ahrs_init();
motors_init();
voltage_reader_start();
//gps_init();
LOG(LL_INFO, "initializing model/controller");
model_init();
ctrl_init();
/* initialize command interface */
LOG(LL_INFO, "initializing cmd interface");
cmd_init();
/* prepare main loop: */
for (int i = 0; i < NUM_AVG; i++)
{
output_avg[i] = sliding_avg_create(OUTPUT_RATIO, 0.0f);
}
LOG(LL_INFO, "system up and running");
struct timespec ts_curr;
struct timespec ts_prev;
struct timespec ts_diff;
clock_gettime(CLOCK_REALTIME, &ts_curr);
/* run model and controller: */
while (1)
{
/* calculate dt: */
ts_prev = ts_curr;
clock_gettime(CLOCK_REALTIME, &ts_curr);
TIMESPEC_SUB(ts_diff, ts_curr, ts_prev);
float dt = (float)ts_diff.tv_sec + (float)ts_diff.tv_nsec / (float)NSEC_PER_SEC;
/* read sensor values into model input structure: */
model_input_t model_input;
model_input.dt = dt;
ahrs_read(&model_input.ahrs_data);
gps_read(&model_input.gps_data);
model_input.ultra_z = ultra_altimeter_read();
model_input.baro_z = baro_altimeter_read();
/* execute model step: */
model_state_t model_state;
model_step(&model_state, &model_input);
/* execute controller step: */
mixer_in_t mixer_in;
ctrl_step(&mixer_in, dt, &model_state);
/* set up mixer input: */
mixer_in.pitch = sliding_avg_calc(output_avg[AVG_PITCH], mixer_in.pitch);
mixer_in.roll = sliding_avg_calc(output_avg[AVG_ROLL], mixer_in.roll);
mixer_in.yaw = sliding_avg_calc(output_avg[AVG_YAW], mixer_in.yaw);
mixer_in.gas = sliding_avg_calc(output_avg[AVG_GAS], mixer_in.gas);
/* write data to motor mixer: */
EVERY_N_TIMES(OUTPUT_RATIO, motors_write(&mixer_in));
}
}
int main (void) {
uint8_t bu, bd; // buttons up, buttons down (read-in buffers)
uint8_t i; // iterator
uint8_t adcchan = 0;
uint8_t ramp;
uint16_t cycle;
motor_t motor[NMOTORS];
for (i = 0; i < NMOTORS; i++) {
motor[i].isrunning = false;
motor[i].position = 0;
motor[i].bu = false;
motor[i].bd = false;
motor[i].pot = 0;
motor[i].trgspeed = SPEEDMIN;
motor[i].curspeed = SPEEDMIN;
motor[i].counter = 0;
}
led_init();
spi_init();
adc_init();
motors_init();
/* // debug */
/* uart_init(); */
/* stdout = &uart_output; */
adc_set_chan(adcchan);
adc_start();
while (1) {
led_off();
// keep the ADC running "in background"
if ( !( adc_is_running() )) {
motor[adcchan].pot = adc_get();
if (motor[adcchan].bu != motor[adcchan].bd) {
motor[adcchan].trgspeed =
motor_scale_speed(motor[adcchan].pot);
} else {
motor[adcchan].trgspeed = SPEEDMIN;
}
adcchan++;
if (adcchan >= 5) adcchan = 0;
// HACK: lame jumpers
if (adcchan == 0) adc_set_chan(6);
if (adcchan == 1) adc_set_chan(1);
if (adcchan == 2) adc_set_chan(2);
if (adcchan == 3) adc_set_chan(3);
if (adcchan == 4) adc_set_chan(7);
adc_start();
}
// read in buttons
shiftreg_load();
bu = spi_transmit(SPI_TRANSMIT_DUMMY);
bd = spi_transmit(SPI_TRANSMIT_DUMMY);
for (i = 0; i < NMOTORS; i++) {
motor[i].bu = bit_is_set(bu, i) ? true : false;
motor[i].bd = bit_is_set(bd, i) ? true : false;
// both buttons pressed
if (motor[i].bu && motor[i].bd) {
led_on();
motor[i].trgspeed = SPEEDMIN;
}
if (motor[i].bu != motor[i].bd) {
motor[i].isrunning = true;
// set dir
if (motor[i].bu) {
motor_set_dir(i, DIR_UP);
} else if (motor[i].bd) {
motor_set_dir(i, DIR_DOWN);
}
}
// allow ramp-down
else if (motor[i].curspeed != motor[i].trgspeed) {
motor[i].isrunning = true;
}
// mark as ok-to-stop
else if (motor[i].curspeed == SPEEDMIN) {
motor[i].isrunning = false;
}
}
for (ramp = 0; ramp < RAMPCYCLES; ramp++) {
// push current speed towards target
for (i = 0; i < NMOTORS; i++) {
if (motor[i].curspeed < motor[i].trgspeed) {
motor[i].curspeed += 1;
}
if (motor[i].curspeed > motor[i].trgspeed) {
motor[i].curspeed -= 1;
}
}
// run at current speed
for (cycle = 0; cycle < RUNCYCLES; cycle++) {
for (i = 0; i < NMOTORS; i++) {
// either button pressed or ramp-down
if (motor[i].isrunning) {
if (motor[i].counter > 0) {
motor[i].counter -= 1;
} else {
// reset counter
motor[i].counter = motor[i].curspeed;
motor_step(i);
}
}
}
}
}
}
return 0;
}
int main( void ){
DDRC = 0xDB;
//PORTC = 0x7E;;
PORTC = 0xE7;
uint8_t error;
Packet packet;
Queue* packets;
packets = Packets_getQueue();
// initialize globals
globals_init();
// initialize Time
time_init();
TimeResult tr;
uint32_t previous = 0;
uint32_t watchdog = 0;
// initialize usart
usart_init();
// initialize motors
motors_init();
motors_set(MOTORS_LEFT, 0);
motors_set(MOTORS_RIGHT, 0);
// initialize analog input
analoginput_init();
// enable interrupts
sei();
// loop forever
while(1){
error = Packets_getError();
if (error){
Packets_sendError(ERROR_PACKETS, error);
continue;
}
if (packets->count > 0){
error = 0x00;
packet = Packets_getNext();
error = Queue_getError(packets);
if (error){
Packets_sendError(ERROR_QUEUE, error);
continue;
}
handle_packet(packet);
watchdog = time_get_time();
}
tr = time_get_time_delta(previous);
// every 20ms, calculate rpm
if (get_time_in_ms(tr.delta) > 20){
previous = tr.previous;
motors_tick();
send_status();
}
tr = time_get_time_delta(watchdog);
if (get_time_in_ms(tr.delta) > 500){
motors_set(MOTORS_LEFT, 0);
motors_set(MOTORS_RIGHT, 0);
}
}
}
int main(int argc, char *argv[]) {
ros::init(argc, argv, "motors");
ros::NodeHandle n;
timerclear(&last_motion);
reset_valid_counts();
/* Node setup */
ros::ServiceServer srv = n.advertiseService("/scout/motion", motion_srv);
ros::Subscriber sub = n.subscribe("/scout/motion", 1, motion_msg);
ros::Publisher pub = n.advertise<scout_msgs::ScoutMotorsMsg>("/scout/motors", 1000);
/* Read parameters */
ros::NodeHandle tmp("~");
if ( !tmp.getParam("accel", accel) )
accel = DEFAULT_ACCEL;
if ( !tmp.getParam("port0", p[0].name) )
p[0].name = DEFAULT_PORT0;
if ( !tmp.getParam("port1", p[1].name) )
p[1].name = DEFAULT_PORT1;
motors_init();
signal(SIGINT, shutdown);
ROS_INFO("Motors node launched (accel=%d)", accel);
while (ros::ok()) {
/* prepare select() */
struct timeval timeout = {0, TIMEOUT_USECS};
fd_set rfds;
FD_ZERO(&rfds);
int maxfd = -1;
for (int i=0 ; i<NP ; i++) {
int fd = p[i].fd;
if (fd>maxfd) maxfd=fd;
FD_SET(fd, &rfds);
}
/* perform select() */
int ret = select(maxfd+1, &rfds, NULL, NULL, &timeout);
if (ret>0) {
/* processing serial port data */
for (int i=0 ; i<NP ; i++) {
int fd = p[i].fd;
if (FD_ISSET(fd, &rfds))
port_process(i);
}
} else {
/* timeout */
ROS_WARN("No response from any motor");
reset_valid_counts();
ask_encoders();
}
/* check of all encoder position reports have arrived */
if (all_valid_counts()) {
/* send ROS message */
motors_msg.header.stamp = ros::Time::now();
motors_msg.count_left = count[0];
motors_msg.count_right = count[1];
/* printf("Sending data %d %d\n", motors_msg.count_left, motors_msg.count_right); */
pub.publish(motors_msg);
reset_valid_counts();
ask_encoders();
}
/* check motor command timeout */
if (timerisset(&last_motion)) {
struct timeval now, diff;
if (-1==gettimeofday(&now, NULL)) {
perror("main:gettimeofday()");
exit(1);
} else {
timersub(&now, &last_motion, &diff);
/* printf("diff: %d %d\n", diff.tv_sec, diff.tv_usec); */
if (diff.tv_sec >= MOTION_TIMEOUT) {
ROS_WARN("No velocity commands for %d seconds -- STOPPING", MOTION_TIMEOUT);
send_vel(0, 0);
timerclear(&last_motion);
}
}
}
/* handle ROS callbacks */
ros::spinOnce();
}
return 0;
}
