// Setup will calibrate our compass by finding maximum/minimum magnetic readings
void Compass::calibrate()
{
_calibrated = false;
// The highest possible magnetic value to read in any direction is 2047
// The lowest possible magnetic value to read in any direction is -2047
LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767};
unsigned char index;
// Initiate the Wire library and join the I2C bus as a master
Wire.begin();
// Initiate LSM303
_compass.init();
// Enables accelerometer and magnetometer
_compass.enableDefault();
_compass.writeReg(LSM303::CRB_REG_M, CRB_REG_M_2_5GAUSS); // +/- 2.5 gauss sensitivity to hopefully avoid overflow problems
_compass.writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ); // 220 Hz compass update rate
delay(500);
LOGi("starting calibration");
// To calibrate the magnetometer, the Zumo spins to find the max/min
// magnetic vectors. This information is used to correct for offsets
// in the magnetometer data.
drive(SPEED, -SPEED);
for(index = 0; index < CALIBRATION_SAMPLES; index ++)
{
// Take a reading of the magnetic vector and store it in compass.m
_compass.read();
running_min.x = min(running_min.x, _compass.m.x);
running_min.y = min(running_min.y, _compass.m.y);
running_max.x = max(running_max.x, _compass.m.x);
running_max.y = max(running_max.y, _compass.m.y);
delay(50);
}
drive_stop();
LOGi("max.x %d", running_max.x);
LOGi("max.y %d", running_max.y);
LOGi("min.x %d", running_min.x);
LOGi("min.y %d", running_min.y);
// Set _calibrated values to compass.m_max and compass.m_min
_compass.m_max.x = running_max.x;
_compass.m_max.y = running_max.y;
_compass.m_min.x = running_min.x;
_compass.m_min.y = running_min.y;
_calibrated = true;
}
void driveIn(uint8_t wheel) {
drive_in = 1;
// Stop roomba
drive_stop();
// Turn the roomba till sensor reaches line
if (wheel == LEFT_WHEEL) {
drive_direction(-DRIVE_STRAIGHT_SPEED, 0);
} else {
drive_direction(0, -DRIVE_STRAIGHT_SPEED);
}
my_msleep(350);
}
void driveInContinued(detectedType activeSensorSide) {
// Stop when sensor readed line
drive_stop();
// Drive into middle of course 30 cm
drive_roomba_exact(300, DRIVE_IN_SPEED);
// Turn to driving direction
if (side == 0) {
drive_turn(90);
} else if (side == 1) {
drive_turn(-90);
}
drive_in = 0;
step = 0;
side = 0;
}
void controlpanel_drive() {
float speed=20;
while (true) {
char ch=controlpanel_promptChar("Drive");
switch (ch) {
case ' ':
drive_stop();
break;
case 'x':
drive_stop(DM_TRAJ);
break;
case 'w':
drive_fd(speed);
break;
case 'a':
drive_lturn(speed);
break;
case 's':
drive_bk(speed);
break;
case 'd':
drive_rturn(speed);
break;
case 'W':
drive_fdDist(speed, 50);
break;
case 'A':
drive_lturnDeg(speed, 90);
break;
case 'S':
drive_bkDist(speed, 50);
break;
case 'D':
drive_rturnDeg(speed, 90);
break;
case '=':
speed += 2;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case '-':
speed -= 2;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case '+':
speed += 10;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case '_':
speed -= 10;
printf_P(PSTR("Speed: %f\n"), speed);
break;
case 'p':
motorcontrol_setDebug(false);
printf_P(PSTR("Debug disabled\n"));
break;
case 'c':
motorcontrol_setEnabled(false);
printf_P(PSTR("Motor control disabled\n"));
break;
case 'P':
motorcontrol_setDebug(true);
break;
case 'q':
motorcontrol_setEnabled(false);
return;
case 'z': // Does moonwalk forwards
speed = 40;
for (int i = 0; i < 20; i++) {
drive_fd(speed);
_delay_ms(25);
drive_rturn(speed);
_delay_ms(50);
drive_fd(speed);
_delay_ms(25);
drive_lturn(speed);
_delay_ms(50);
}
drive_stop();
speed = 20;
break;
case 'Z': // Does moonwalk backwards
speed = 100;
for (int i = 0; i < 20; i++) {
drive_bk(speed);
_delay_ms(25);
drive_rturn(speed);
_delay_ms(50);
drive_bk(speed);
_delay_ms(25);
drive_lturn(speed);
_delay_ms(50);
}
speed = 20;
break;
case 'm': {
float amax = drive_getTrajAmax();
printf_P(PSTR("Current amax: %.2f\n"), amax);
if (controlpanel_prompt("amax", "%f", &amax) != 1) {
printf_P(PSTR("Cancelled.\n"));
continue;
}
drive_setTrajAmax(amax);
printf_P(PSTR("Amax set to: %.2f\n"), amax);
break;
}
default:
puts_P(unknown_str);
drive_stop();
break;
case '?':
static const char msg[] PROGMEM =
"Drive commands:\n"
" wasd - Control robot\n"
" space - Stop\n"
" -=_+ - Adjust speed\n"
" WASD - Execute distance moves\n"
" c - Disable motor control\n"
" Pp - Enable/Disable motor control debug\n"
" zZ - Moonwalk (WIP)\n"
" q - Back";
puts_P(msg);
break;
}
}
}
void controlpanel_drive() {
int16_t steer = 0;
while (true) {
char ch=controlpanel_promptChar("Drive");
switch (ch) {
case ' ':
drive_stop();
speed = 0;
steer = 0;
break;
case 'w':
steer = 0;
speed += speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed);
break;
case 'a':
steer -= steer_incrementer;
drive_steer(steer, speed);
break;
case 's':
steer = 0;
speed -= speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed); // speed will be negative!!
break;
case 'd':
steer += steer_incrementer;
drive_steer(steer, speed);
break;
case 'W':
steer = 0;
speed += large_speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed);
break;
case 'A':
drive_lturn(setSpeed - turn_reducer);
break;
case 'S':
steer = 0;
speed -= large_speed_incrementer;
printf_P(PSTR("Speed: %f\n"), speed);
drive_fd(speed); // speed will be negative!!
break;
case 'D':
drive_rturn(setSpeed - turn_reducer);
break;
case 'k':
weedwhacker_power(false);
break;
case 'K':
weedwhacker_power(true);
break;
case '=':
setSpeed += 100;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case '-':
setSpeed -= 100;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case '+':
setSpeed += 10;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case '_':
setSpeed -= 10;
printf_P(PSTR("Speed: %f\n"), setSpeed);
break;
case 'p':
motorcontrol_setDebug(false);
printf_P(PSTR("Debug disabled\n"));
break;
case 'c':
motorcontrol_setEnabled(false);
printf_P(PSTR("Motor control disabled\n"));
break;
case 'P':
motorcontrol_setDebug(true);
break;
case 'q':
// motorcontrol_setEnabled(false);
return;
case 'm': {
/*float amax = drive_getTrajAmax();
printf_P(PSTR("Current amax: %.2f\n"), amax);
if (controlpanel_prompt("amax", "%f", &amax) != 1) {
printf_P(PSTR("Cancelled.\n"));
continue;
}
drive_setTrajAmax(amax);
printf_P(PSTR("Amax set to: %.2f\n"), amax);*/
break;
}
default:
puts_P(unknown_str);
drive_stop();
break;
case '?':
static const char msg[] PROGMEM =
"Drive commands:\n"
" wasd - Control robot\n"
" space - Stop\n"
" k - Weedwhacker Kill\n"
" K - Weedwhacker Start\n"
" -=_+ - Adjust speed\n"
" WASD - Full speed movements\n"
" c - Disable motor control\n"
" Pp - Enable/Disable motor control debug\n"
" q - Back";
puts_P(msg);
break;
}
}
}
int main(void) {
//setting PWM-Ports as output
DDRB = 0xFF; //set port B as output
DDRD = 0b11111101; //set port D as output with pin D1 input
//setup the pwm for the servo
TCCR1A = _BV(COM1A1) // set OC1A/B at TOP
| _BV(COM1B1) // clear OC1A/B when match
| _BV(WGM11); //(fast PWM, clear TCNT1 on match ICR1)
TCCR1B = _BV(WGM13)
| _BV(WGM12)
| _BV(CS11); // timer uses main system clock with 1/8 prescale
ICR1 = 20000; // used for TOP, makes for 50 hz PWM
OCR1A = 1500; // servo at center
//set up pwm for the motors
TCCR0A = _BV(COM0A1) // set OC1A/B at TOP
| _BV(COM0B1) // clear OC1A/B when match
| _BV(WGM00) // fast PWM mode
| _BV(WGM01);
TCCR0B = _BV(CS01)
| _BV(CS00);
drive_stop(0); //motors stopped
servo_position(servo_center, 50); //servo center
//variables to hold the distances;
uint16_t forward_dist;
uint16_t left_dist;
uint16_t right_dist;
uint8_t fwd_count;
servo_position(servo_center, 50);
while(1)
{
forward_dist = sensor_distance();
_delay_ms(50);
if (forward_dist <= forward_buffer)
{
drive_stop(0);
beep_meow();
servo_position(servo_left, 400);
left_dist = sensor_distance();
servo_position(servo_right,400);
right_dist = sensor_distance();
servo_position(servo_center,200);
while ((left_dist > right_dist) && (forward_dist <= (forward_buffer + now_clear)))
{
drive_left(speed_80p);
forward_dist = sensor_distance();
_delay_ms(50);
}
while ((left_dist <= right_dist) && (forward_dist <= (forward_buffer + now_clear)))
{
drive_right(speed_80p);
forward_dist = sensor_distance();
_delay_ms(50);
}
}
drive_forward(speed_80p);
fwd_count++;
if (fwd_count == 20)
{
fwd_count = 0;
servo_position(servo_left, 150);
left_dist = sensor_distance();
if (left_dist <= sides_buffer)
{
drive_stop(0);
beep_meow();
while (left_dist <= (sides_buffer + now_clear))
{
drive_right(speed_80p);
left_dist = sensor_distance();
_delay_ms(100);
}
}
servo_position(servo_center,100);
servo_position(servo_right, 150);
right_dist = sensor_distance();
if (right_dist <= sides_buffer)
{
drive_stop(0);
beep_meow();
while (right_dist <= (sides_buffer + now_clear))
{
drive_left(speed_80p);
right_dist = sensor_distance();
_delay_ms(100);
}
}
servo_position(servo_center,100);
}
}
return 0;
}
