void right() {
set_motor_speed(0,0);
set_motor_speed(1,SPEED);
do {
read_sonar(3,&(dist[3]));
} while (dist[3] <= LIMIAR);
set_motor_speed(1,SPEED);
set_motor_speed(0,SPEED);
}
void turnleft(int degrees)
{
float coefficient = 1.0;
set_motor_speed(pin, 1000);
set_motor_speed(pin, 2000);
delay(coefficient * degrees);
set_motor_speed(pin, 1500);
set_motor_speed(pin, 1500);
}
void left() {
set_motor_speed(1,0);
set_motor_speed(0,SPEED);
do {
read_sonar(4,&(dist[4]));
} while (dist[4] <= LIMIAR);
set_motor_speed(1,SPEED);
set_motor_speed(0,SPEED);
}
/* O robo anda em linha reta ate encontrar uma parede e
entao passa a seguir a parede estando com ela sempre
em seu lado direito */
int main() {
unsigned short *dist[16];
/* inicia no modo busca-parede */
mode = 0;
/* cria proximity call back para quando estiver proximo da parede */
register_proximity_callback(3, MIM_DIST, found);
register_proximity_callback(4, MIM_DIST, found);
/* comeca andando em linha reta */
set_motors_speed(SPEED, SPEED);
/* enquanto estiver no modo busca-parede */
while(mode == 0);
/* passa a andar para esquerda */
set_motors_speed(HSPEED, 0);
/* enquanto nao estiver de lado para parede */
do {
read_sonar(3, dist[3]);
read_sonar(4, dist[4]);
} while(*(dist[3]) <= MIM_DIST && *(dist[4]) <= MIM_DIST);
/* volta a andar para frente */
set_motors_speed(SPEED, SPEED);
dir = 0;
/* fica seguindo a parede */
while(mode) {
read_sonar(7, dist[7]);
read_sonar(8, dist[8]);
if ( *(dist[7]) <= MIM_DIST || *(dist[8]) <= MIM_DIST ) {
if(dir != 2) {
set_motor_speed(1, HSPEED);
}
} else if ( *(dist[7]) >= MAX_DIST || *(dist[8]) >= MAX_DIST ) {
if(dir != 1) {
set_motor_speed(0, HSPEED);
}
} else {
if(dir != 0) {
set_motors_speed(SPEED, SPEED);
}
}
}
return 0;
}
void motor_l_fwd(int spd){
//P1OUT |= MOTOR_L_A;
//P1OUT &= ~MOTOR_L_B;
set_motor_speed(0,spd);
}
static int set_motor_speed_arg(int argc, char *argv[], shared_data_t *data)
{
long int speed_m1, speed_m2;
char *end;
if (argc != 3) {
syslog(LOG_ERR, "Motor speed RPC: too few arguments *ms <-100-100> <-100-100>*\n");
return -1;
}
speed_m1 = strtol(argv[1], &end, 10);
if (speed_m1 < -100 || speed_m1 > 100 || *end != '\0') {
syslog(LOG_ERR, "Motor speed RPC: invalid argument 1 %s",
argv[1]);
return -1;
}
speed_m2 = strtol(argv[2], &end, 10);
if (speed_m1 < -100 || speed_m1 > 100 || *end != '\0') {
syslog(LOG_ERR, "Motor speed RPC: invalid argument 2 %s",
argv[2]);
return -1;
}
return set_motor_speed(data, (int)speed_m1, (int)speed_m2);
}
void Robot_track_end(void)
{
WriteTimer0(0);
while((ReadTimer0()<40000) && (SeeLine.B==0))
{
set_motor_speed(left, medium, 0);
set_motor_speed(right, medium, 0);
}
if(ReadTimer0()>40000 )
{
while(SeeLine.B==0)
{
set_motor_speed(left, rev_slow, 0);
set_motor_speed(right, rev_slow, 0);
}
}
}
void update_all_motors_speed(void){
if (motor_1_speed>MAX_SPEED){
motor_1_speed=MAX_SPEED;
}
if (motor_2_speed>MAX_SPEED){
motor_2_speed=MAX_SPEED;
}
if (motor_3_speed>MAX_SPEED){
motor_3_speed=MAX_SPEED;
}
if (motor_4_speed>MAX_SPEED){
motor_4_speed=MAX_SPEED;
}
set_motor_speed (1,motor_1_speed);
set_motor_speed (2,motor_2_speed);
set_motor_speed (3,motor_3_speed);
set_motor_speed (4,motor_4_speed);
}
void idle(void)
{
color_t c;
uint8_t animate = 0, current_state = 0, button_state_changed = 0;
uint32_t t = 0;
cli();
if (g_sync_count >= g_sync_divisor)
{
g_sync_count = 0;
animate = 1;
}
// read button state & check time
if (g_button_time > 0 && g_time >= g_button_time)
{
current_state = g_button_state;
g_button_time = 0;
button_state_changed = 1;
}
sei();
// Set the leds and motor speed accordingly when button is pressed
if (button_state_changed)
{
if (current_state)
set_motor_speed(255, 1);
else
set_motor_speed(0, 1);
}
// run the animation if the current state
if (animate)
{
cli();
t = g_pattern_t++;
sei();
// do some animation!
led_pattern_next(t, &c);
set_led_rgb(c.red, c.green, c.blue);
}
flush_saved_tick_count(0);
}
void run_motor_timed(uint32_t duration)
{
uint32_t t;
if (duration == 0)
return;
set_motor_speed(255, 1);
for(t = 0; t < duration && !check_reset(); t++)
_delay_ms(1);
stop_motor();
}
static int init_motor(shared_data_t *data)
{
pinMode(motor_1.in_1, OUTPUT);
pinMode(motor_1.in_2, OUTPUT);
pinMode(motor_2.in_1, OUTPUT);
pinMode(motor_2.in_2, OUTPUT);
set_motor_speed(data, 0, 0);
motor_switch_direction(motor_1);
motor_switch_direction(motor_2);
return 0;
}
void robot_test_left(void)
{
set_motor_speed(left, slow, 0);
delay_1S(1);
set_motor_speed(left, medium, 0);
delay_1S(1);
set_motor_speed(left, fast, 0);
delay_1S(1);
set_motor_speed(left, stop, 0);
delay_1S(1);
set_motor_speed(left, rev_slow, 0);
delay_1S(1);
set_motor_speed(left, rev_medium, 0);
delay_1S(1);
set_motor_speed(left, rev_fast, 0);
delay_1S(1);
set_motor_speed(left, stop, 0);
}
void dispense_ticks(uint32_t ticks, uint16_t speed)
{
uint8_t dispensing;
cli();
dispensing = g_is_dispensing;
sei();
if (dispensing || ticks == 0)
return;
cli();
g_dispense_target_ticks = g_ticks + ticks;
g_is_dispensing = 1;
sei();
set_motor_speed(speed, 1);
}
// initialise sensor (returns true if sensor is succesfully initialised)
bool AP_Proximity_LightWareSF40C::initialise()
{
// set motor direction once per second
if (_motor_direction > 1) {
if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
set_motor_direction();
}
}
// set forward direction once per second
if (_forward_direction != frontend.get_yaw_correction(state.instance)) {
if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
set_forward_direction();
}
}
// request motors turn on once per second
if (_motor_speed == 0) {
if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
set_motor_speed(true);
}
return false;
}
return true;
}
void move(signed char speed) {
set_motor_speed(1, speed);
set_motor_speed(2, -speed);
}
void turn_right(void)
{
set_motor_speed(left, fast, 0);
set_motor_speed(right, stop, 0);
}
void spin_right(void)
{
set_motor_speed(left, fast, 0);
set_motor_speed(right, rev_fast, 0);
}
void straight_fwd(void)
{
set_motor_speed(left, fast, 0);
set_motor_speed(right, fast, 0);
}
// ----------------------------------------------------------------------------------------------------------------------
void init_main_board() {
// HORN
*(&HORN_PORT_reg - 1) |= _BV(HORN_PIN_bit); // set pin to output
// HEAD LIGHT
*(&HEAD_LIGHT_PORT_reg - 1) |= _BV(HEAD_LIGHT_PIN_bit); // set pin to output
// BRAKE LIGHT
*(&BRAKE_LIGHT_PORT_reg - 1) |= _BV(BRAKE_LIGHT_PIN_bit); // set pin to output
// AUX
*(&AUX_PORT_reg - 1) |= _BV(AUX_PIN_bit); // set pin to output
// MOTOR SPEED Fast-PWM ICR = TOP setup
// Mode 14
MOTOR_CONTROL_TCCRA_reg |= _BV(MOTOR_CONTROL_WGM1_bit);
MOTOR_CONTROL_TCCRB_reg |= _BV(MOTOR_CONTROL_WGM2_bit) | _BV(MOTOR_CONTROL_WGM3_bit);
// Set OCA on compare match and clear on BOTTOM
MOTOR_CONTROL_OCRA_reg = MOTOR_CONTROL_TOP;
MOTOR_CONTROL_TCCRA_reg |= _BV(MOTOR_CONTROL_COMA1_bit) | _BV(MOTOR_CONTROL_COMA0_bit);
*(&MOTOR_CONTROL_OCA_PORT_reg - 1) |= _BV(MOTOR_CONTROL_OCA_PIN_bit); // set pin to output
// Set OCB on compare match and clear on BOTTOM
MOTOR_CONTROL_OCRB_reg = MOTOR_CONTROL_TOP;
MOTOR_CONTROL_TCCRA_reg |= _BV(MOTOR_CONTROL_COMB1_bit) | _BV(MOTOR_CONTROL_COMB0_bit);
*(&MOTOR_CONTROL_OCB_PORT_reg - 1) |= _BV(MOTOR_CONTROL_OCB_PIN_bit); // set pin to output
// PWM Freq set to 25 KHz
// TOP = F_CPU/(Fpwm*N)-1
MOTOR_CONTROL_ICR_reg = MOTOR_CONTROL_TOP;
// Set prescaler and start timer
#if (MOTOR_CONTROL_PRESCALER == 1)
MOTOR_CONTROL_TCCRB_reg |= _BV(MOTOR_CONTROL_CS0_bit); // Prescaler 1 and Start Timer
#elif ((MOTOR_CONTROL_PRESCALER == 8))
MOTOR_CONTROL_TCCRB_reg |= _BV(MOTOR_CONTROL_CS1_bit); // Prescaler 8 and Start Timer
#elif ((MOTOR_CONTROL_PRESCALER == 64))
MOTOR_CONTROL_TCCRB_reg |= _BV(MOTOR_CONTROL_CS0_bit) | _BV(MOTOR_CONTROL_CS1_bit); // Prescaler 64 and Start Timer
#elif ((MOTOR_CONTROL_PRESCALER == 256))
MOTOR_CONTROL_TCCRB_reg |= _BV(MOTOR_CONTROL_CS2_bit); // Prescaler 256 and Start Timer
#elif ((MOTOR_CONTROL_PRESCALER == 1024))
MOTOR_CONTROL_TCCRB_reg |= _BV(MOTOR_CONTROL_CS0_bit) | _BV(MOTOR_CONTROL_CS2_bit); ; // Prescaler 1024 and Start Timer
#endif
set_motor_speed(0);
// TACHO Counter
// External Clock source - Falling Edge
TACHO_TCCRB_reg |= _BV(TACHO_CS2_bit) | _BV(TACHO_CS1_bit);
// Bluetooth
*(&BT_RTS_PORT - 1) &= ~_BV(BT_RTS_PIN); // set pin to input
*(&BT_CTS_PORT - 1) |= _BV(BT_CTS_PIN); // set pin to output
BT_CTS_PORT &= ~_BV(BT_CTS_PIN); // Set CTS pin Low
*(&BT_RESET_PORT - 1) |= _BV(BT_RESET_PIN); // set pin to output
BT_RESET_PORT &= ~_BV(BT_RESET_PIN); // Set RESET Low/active
*(&BT_AUTO_DISCOVERY_PORT - 1) |= _BV(BT_AUTO_DISCOVERY_PIN); // set pin to output
BT_AUTO_DISCOVERY_PORT &= ~_BV(BT_AUTO_DISCOVERY_PIN); // Set AUTO_DISCOVERY Low/disable
*(&BT_MASTER_PORT - 1) |= _BV(BT_MASTER_PIN); // set pin to output
BT_MASTER_PORT &= ~_BV(BT_MASTER_PIN); // Set BT_MASTER Low/client mode
// Enable goal-line interrupt - INT0 falling edge
EICRA |= _BV(ISC01);
EIMSK |= _BV(INT0);
static buffer_struct_t _bt_rx_buffer;
static buffer_struct_t _bt_tx_buffer;
buffer_init(&_bt_rx_buffer);
buffer_init(&_bt_tx_buffer);
_bt_serial_instance = serial_new_instance(ser_USART0, 9600UL, ser_BITS_8, ser_STOP_1, ser_NO_PARITY, &_bt_rx_buffer, &_bt_tx_buffer, _bt_call_back);
_init_mpu9520();
_init_dialog_handler_timer();
}
int main(void)
{
uint8_t id, rec, i, cs;
color_t c;
packet_t p;
setup();
stop_motor();
sei();
for(i = 0; i < 5; i++)
{
set_led_rgb(255, 0, 255);
_delay_ms(50);
set_led_rgb(255, 255, 0);
_delay_ms(50);
}
// get the current liquid level
update_liquid_level();
for(;;)
{
cli();
g_reset = 0;
g_current_sense_detected = 0;
g_current_sense_num_cycles = 0;
setup();
serial_init();
stop_motor();
set_led_rgb(0, 0, 255);
sei();
id = address_exchange();
for(; !check_reset();)
{
rec = receive_packet(&p);
if (rec == COMM_CRC_FAIL)
continue;
if (rec == COMM_RESET)
break;
if (rec == COMM_OK && (p.dest == DEST_BROADCAST || p.dest == id))
{
// If we've detected a over current sitatuion, ignore all comamnds until reset
cli();
cs = g_current_sense_detected;
sei();
switch(p.type)
{
case PACKET_PING:
break;
case PACKET_SET_MOTOR_SPEED:
if (!cs)
set_motor_speed(p.p.uint8[0], p.p.uint8[1]);
if (p.p.uint8[0] == 0)
flush_saved_tick_count(0);
break;
case PACKET_TICK_DISPENSE:
if (!cs)
{
dispense_ticks((uint16_t)p.p.uint32, 255);
flush_saved_tick_count(0);
}
break;
case PACKET_TIME_DISPENSE:
if (!cs)
{
run_motor_timed(p.p.uint32);
flush_saved_tick_count(0);
}
break;
case PACKET_IS_DISPENSING:
is_dispensing();
break;
case PACKET_LIQUID_LEVEL:
get_liquid_level();
break;
case PACKET_UPDATE_LIQUID_LEVEL:
update_liquid_level();
break;
case PACKET_LED_OFF:
set_led_pattern(LED_PATTERN_OFF);
break;
case PACKET_LED_IDLE:
if (!cs)
set_led_pattern(LED_PATTERN_IDLE);
break;
case PACKET_LED_DISPENSE:
if (!cs)
set_led_pattern(LED_PATTERN_DISPENSE);
break;
case PACKET_LED_DRINK_DONE:
if (!cs)
set_led_pattern(LED_PATTERN_DRINK_DONE);
break;
case PACKET_LED_CLEAN:
if (!cs)
set_led_pattern(LED_PATTERN_CLEAN);
break;
case PACKET_COMM_TEST:
comm_test();
break;
case PACKET_ID_CONFLICT:
id_conflict();
break;
case PACKET_SET_CS_THRESHOLD:
g_current_sense_threshold = p.p.uint16[0];
break;
case PACKET_SAVED_TICK_COUNT:
get_saved_tick_count();
break;
case PACKET_RESET_SAVED_TICK_COUNT:
reset_saved_tick_count();
break;
case PACKET_FLUSH_SAVED_TICK_COUNT:
flush_saved_tick_count(1);
break;
case PACKET_GET_LIQUID_THRESHOLDS:
get_liquid_thresholds();
break;
case PACKET_SET_LIQUID_THRESHOLDS:
set_liquid_thresholds(p.p.uint16[0], p.p.uint16[1]);
break;
case PACKET_TICK_SPEED_DISPENSE:
if (!cs)
{
dispense_ticks(p.p.uint16[0], (uint8_t)p.p.uint16[1]);
flush_saved_tick_count(0);
}
break;
case PACKET_PATTERN_DEFINE:
pattern_define(p.p.uint8[0]);
break;
case PACKET_PATTERN_ADD_SEGMENT:
c.red = p.p.uint8[0];
c.green = p.p.uint8[1];
c.blue = p.p.uint8[2];
pattern_add_segment(&c, p.p.uint8[3]);
break;
case PACKET_PATTERN_FINISH:
pattern_finish();
break;
}
}
}
}
return 0;
}
void motor_r_fwd(int spd){
//P1OUT |= MOTOR_R_A;
//P1OUT &= ~MOTOR_R_B;
set_motor_speed(1,spd);
}
void reverse(void)
{
set_motor_speed(pin, 1000);
set_motor_speed(pin2,1000);
}
void set_motor_speeds(int speed_left, int speed_right){
set_motor_speed(0, speed_left);
set_motor_speed(1, speed_right);
}
void main(void){
//------------------------------------------------------------------------------
// Main Program
// This is the main routine for the program. Execution of code starts here.
// The operating system is Back Ground Fore Ground.
//
//------------------------------------------------------------------------------
init_ports();
Init_Clocks();
Init_Conditions();
init_timers();
five_msec_delay(QUARTER_SECOND);
Init_LCD();
setup_sw_debounce();
init_adc();
P1OUT |= IR_LED;
init_serial_uart();
WDTCTL = WDTPW + WDTHOLD;
setup_pwm();
set_motor_speed(R_FORWARD, PWM_RES);
set_motor_speed(L_FORWARD, PWM_RES);
unsigned int time_sequence = START_VAL; // counter for switch loop
unsigned int previous_count = START_VAL; // automatic variable for
// comparing timer_count
unsigned int display_count = START_VAL;
is_follow_running = FALSE;
//------------------------------------------------------------------------------
// Begining of the "While" Operating System
//------------------------------------------------------------------------------
while(ALWAYS) { // Can the Operating system run
if(get_timer_count() > display_count + QUARTER_SECOND)
{
display_count = get_timer_count();
Display_Process();
time_sequence = START_VAL;
}
update_switches(); // Check for switch state change
update_menu();
if(is_follow_running)
run_follow();
if(uca0_is_message_received())
{
BufferString message = uca0_read_buffer(TRUE);
receive_command(message.head + message.offset);
}
if(uca1_is_message_received())
{
update_menu();
BufferString message = uca1_read_buffer(TRUE);
uca0_transmit_message(message.head, message.offset);
if(find(WIFI_COMMAND_SYMBOL, message))
{
receive_command(message.head + message.offset);
}
if(find(LOST_WIFI_COMMAND_SYMBOL, message))
{
receive_command(CONNECT_NCSU);
}
}
if(time_sequence > SECOND_AND_A_QUARTER)
time_sequence = START_VAL;
unsigned int current_timer_count = get_timer_count();
if(current_timer_count > previous_count)
{
previous_count = current_timer_count % UINT_16_MAX;
time_sequence++;
}
}
//------------------------------------------------------------------------------
}
void motor_r_rev(int spd){
//P1OUT |= MOTOR_R_B;
//P1OUT &= ~MOTOR_R_A;
spd *= -1;//make negative
set_motor_speed(1,spd);
}
void stop() {
set_motor_speed(1,0);
set_motor_speed(0,0);
}
static void set_motor_speeds(int32_t leftspeed, int32_t rightspeed)
{
set_motor_speed(LEFT, leftspeed);
set_motor_speed(RIGHT, rightspeed);
}
void text_interface(void)
{
char cmd[MAX_CMD_LEN];
uint8_t speed, current_sense;
uint16_t ticks;
uint16_t t;
uint8_t i, cs;
for(i = 0; i < 5; i++)
{
set_led_rgb(0, 0, 255);
_delay_ms(150);
set_led_rgb(0, 0, 0);
_delay_ms(150);
}
set_led_pattern(LED_PATTERN_IDLE);
for(;;)
{
cli();
g_reset = 0;
g_current_sense_detected = 0;
setup();
stop_motor();
serial_init();
cs = 0;
sei();
_delay_ms(10);
dprintf("\nParty Robotics Dispenser at your service!\n\n");
for(;;)
{
cli();
cs = g_current_sense_detected;
sei();
if (!receive_cmd(cmd))
break;
if (sscanf(cmd, "speed %hhu %hhu", &speed, ¤t_sense) == 2)
{
if (!cs)
set_motor_speed(speed, current_sense);
if (current_sense == 0)
flush_saved_tick_count(0);
continue;
}
if (sscanf(cmd, "tickdisp %hu %hhu", (short unsigned int *)&ticks, &speed) == 2)
{
if (!cs)
{
dispense_ticks(ticks, speed);
flush_saved_tick_count(0);
}
continue;
}
if (sscanf(cmd, "timedisp %hu", (short unsigned int *)&t) == 1)
{
if (!cs)
{
run_motor_timed(t);
flush_saved_tick_count(0);
}
continue;
}
if (strncmp(cmd, "forward", 7) == 0)
{
set_motor_direction(MOTOR_DIRECTION_FORWARD);
continue;
}
if (strncmp(cmd, "backward", 8) == 0)
{
set_motor_direction(MOTOR_DIRECTION_BACKWARD);
continue;
}
if (strncmp(cmd, "led_idle", 8) == 0)
{
set_led_pattern(LED_PATTERN_IDLE);
continue;
}
if (strncmp(cmd, "led_dispense", 12) == 0)
{
set_led_pattern(LED_PATTERN_DISPENSE);
continue;
}
if (strncmp(cmd, "led_done", 8) == 0)
{
set_led_pattern(LED_PATTERN_DRINK_DONE);
continue;
}
if (strncmp(cmd, "led_clean", 8) == 0)
{
set_led_pattern(LED_PATTERN_CLEAN);
continue;
}
if (strncmp(cmd, "help", 4) == 0)
{
dprintf("You can use these commands:\n");
dprintf(" speed <speed> <cs>\n");
dprintf(" tickdisp <ticks> <speed>\n");
dprintf(" timedisp <ms>\n");
dprintf(" forward\n");
dprintf(" backward\n");
dprintf(" reset\n");
dprintf(" led_idle\n");
dprintf(" led_dispense\n");
dprintf(" led_done\n");
dprintf(" led_clean\n\n");
dprintf("speed is from 0 - 255. cs = current sense and is 0 or 1.\n");
dprintf("ticks == number of quarter turns. ms == milliseconds\n");
continue;
}
if (strncmp(cmd, "reset", 5) == 0)
break;
dprintf("Unknown command. Use help to get, eh help. Duh.\n");
}
}
}
static void deinit_motor(shared_data_t *data)
{
set_motor_speed(data, 0, 0);
}
void forward(void)
{
set_motor_speed(pin, 2000);
set_motor_speed(pin2,2000);
}
