Skip to content

Commit

Permalink
modify calibration and ohc
Browse files Browse the repository at this point in the history
  • Loading branch information
@acornejo
acornejo committed Jun 11, 2014
1 parent 3647e98 commit 8a15c1d
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 50 deletions.
31 changes: 16 additions & 15 deletions kilolib.c
View file
Expand Up @@ -264,26 +264,23 @@ static inline void process_message() {
}
break;
case CALIB:
if (calibmsg->mode != CALIB_SAVE && calibmsg->mode != CALIB_UID) {
if (kilo_state != MOVING) {
motors_on();
kilo_state = MOVING;
}
} else {
motors_off();
kilo_state = IDLE;
}
reset();
switch(calibmsg->mode) {
case CALIB_SAVE:
eeprom_write_byte(EEPROM_UID, kilo_uid&0xFF);
eeprom_write_byte(EEPROM_UID+1, (kilo_uid>>8)&0xFF);
eeprom_write_byte(EEPROM_LEFT_ROTATE, kilo_turn_left);
eeprom_write_byte(EEPROM_RIGHT_ROTATE, kilo_turn_right);
eeprom_write_byte(EEPROM_LEFT_STRAIGHT, kilo_straight_left);
eeprom_write_byte(EEPROM_RIGHT_STRAIGHT, kilo_straight_right);
if (kilo_state == MOVING) {
eeprom_write_byte(EEPROM_UID, kilo_uid&0xFF);
eeprom_write_byte(EEPROM_UID+1, (kilo_uid>>8)&0xFF);
eeprom_write_byte(EEPROM_LEFT_ROTATE, kilo_turn_left);
eeprom_write_byte(EEPROM_RIGHT_ROTATE, kilo_turn_right);
eeprom_write_byte(EEPROM_LEFT_STRAIGHT, kilo_straight_left);
eeprom_write_byte(EEPROM_RIGHT_STRAIGHT, kilo_straight_right);
motors_off();
kilo_state = IDLE;
}
break;
case CALIB_UID:
kilo_uid = calibmsg->uid;
cur_motion = MOVE_STOP;
break;
case CALIB_TURN_LEFT:
if (cur_motion != MOVE_LEFT || kilo_turn_left != calibmsg->turn_left) {
Expand All @@ -308,6 +305,10 @@ static inline void process_message() {
}
break;
}
if (calibmsg->mode != CALIB_SAVE && kilo_state != MOVING) {
motors_on();
kilo_state = MOVING;
}
break;
case READUID:
if (kilo_state != MOVING) {
Expand Down
58 changes: 23 additions & 35 deletions ohc.c
View file
Expand Up @@ -14,7 +14,7 @@ uint8_t packet_checksum = 0;
uint8_t new_packet[PACKET_SIZE];
volatile uint8_t packet_type;
volatile uint8_t has_new_packet = 0;
volatile uint8_t tx_mask;
volatile uint8_t tx_mask = 0;
uint8_t leds_toggle = 0;
message_t msg;
bootmsg_t *bootmsg;
Expand All @@ -24,33 +24,25 @@ gpsmsg_t *gpsmsg;
#define ir_port PORTB
#define ir_ddr DDRB
#define ir_mask (1<<1)
#define blue_port PORTB
#define blue_ddr DDRB
#define blue_mask (1<<5)
#define green_port PORTB
#define green_ddr DDRB
#define green_mask (1<<5)
#define led_port PORTB
#define led_ddr DDRB
#define led_mask (1<<5)
#else
#define ir_port PORTD
#define ir_ddr DDRD
#define ir_mask (1<<3)
#define blue_port PORTD
#define blue_ddr DDRD
#define blue_mask (1<<2)
#define green_port PORTB
#define green_ddr DDRB
#define green_mask (1<<1)
#define led_port PORTB
#define led_ddr DDRB
#define led_mask (1<<1)
#endif

int main() {
cli();
// Set port outputs
ir_ddr |= ir_mask;
green_ddr |= green_mask;
blue_ddr |= blue_mask;
led_ddr |= led_mask;
// Turn off all leds
green_port &= ~green_mask;
blue_port &= ~blue_mask;
led_port &= ~led_mask;
ir_port &= ~ir_mask;
// turn off analog comparator (to avoid detecting collisions)
ACSR |= (1<<ACD);
Expand Down Expand Up @@ -84,11 +76,9 @@ int main() {
// Use LEDs to flash power on indicator signal.
uint8_t i;
for (i=0; i<5; i++) {
blue_port |= blue_mask;
green_port |= green_mask;
led_port |= led_mask;
_delay_ms(200);
blue_port &= ~blue_mask;
green_port &= ~green_mask;
led_port &= ~led_mask;
_delay_ms(200);
}

Expand All @@ -101,11 +91,9 @@ int main() {
case PACKET_LEDTOGGLE:
leds_toggle = !leds_toggle;
if (leds_toggle) {
blue_port |= blue_mask;
green_port |= green_mask;
led_port |= led_mask;
} else {
blue_port &= ~blue_mask;
green_port &= ~green_mask;
led_port &= ~led_mask;
}
break;
case PACKET_FORWARDMSG:
Expand All @@ -114,9 +102,9 @@ int main() {
msg.crc = message_crc(&msg);
while(!has_new_packet) {
message_send(&msg);
green_port |= green_mask;
led_port |= led_mask;
_delay_ms(3);
green_port &= ~green_mask;
led_port &= ~led_mask;
_delay_ms(3);
}
break;
Expand All @@ -125,19 +113,19 @@ int main() {
msg.rawdata[i] = new_packet[i+2];
msg.crc = message_crc(&msg);
message_send(&msg);
green_port |= green_mask;
led_port |= led_mask;
_delay_ms(3);
green_port &= ~green_mask;
led_port &= ~led_mask;
_delay_ms(3);
break;
case PACKET_FORWARDRAWMSG:
for (i = 0; i<sizeof(message_t); i++)
msg.rawdata[i] = new_packet[i+2];
while(!has_new_packet) {
message_send(&msg);
green_port |= green_mask;
led_port |= led_mask;
_delay_ms(3);
green_port &= ~green_mask;
led_port &= ~led_mask;
_delay_ms(3);
}
break;
Expand All @@ -153,9 +141,9 @@ int main() {
message_send(&msg);
}
sei();
green_port |= green_mask;
led_port |= led_mask;
_delay_ms(10);
green_port &= ~green_mask;
led_port &= ~led_mask;
_delay_ms(10);
break;
#define GPS_MSGSIZE 8
Expand All @@ -172,9 +160,9 @@ int main() {
_delay_us(50);
}
sei();
green_port |= green_mask;
led_port |= led_mask;
_delay_ms(10);
green_port &= ~green_mask;
led_port &= ~led_mask;
_delay_ms(10);
break;
}
Expand Down

0 comments on commit 8a15c1d

Please sign in to comment.