void swing_left(int speed, int interval)
{
set_motor_power(0, -speed);
delay_ms(interval);
set_motor_power(0, 0);
set_motor_power(1, 0);
}
void swing_right_r(int speed, int interval)
{
set_motor_power(0, -speed+75);
set_motor_power(1, -speed);
delay_ms(interval);
set_motor_power(0, 0);
set_motor_power(1, 0);
}
void pivot_right(int speed, int interval)
{
set_motor_power(0, speed);
set_motor_power(1, -speed);
delay_ms(interval);
set_motor_power(0, 0);
set_motor_power(1, 0);
}
void reverse(int speed ,int interval)
{
set_motor_power(1, -speed);
set_motor_power(0, -speed);
delay_ms(interval);
set_motor_power(0, 0);
set_motor_power(1, 0);
}
int move( s08 wheel1, s08 wheel2 )
{
// LEFT_WHEEL and RIGHTWHEEL are set above in the #define section
set_motor_power( LEFT_WHEEL, wheel1 );
set_motor_power( RIGHT_WHEEL, -wheel2 ); // this motor is facing the opposite direction from the
// other motor so it must go in the opposite direction
return 0;
}
void forward(int speed)
{
int speed2=speed;
int right=set_motor_power(0, speed-5);
int left =set_motor_power(1, speed);
delay_ms(100);
}
int main(void)
{
struct heading result;
/*u08 k=0;
u08 b=0;
u08 choice=0;
u08 button=0;*/
u08 ir=0;
u08 i=0;
x = 64;
y = 48;
initialize();
motor_init();
servo_init();
set_motor_power(0,0);
set_motor_power(1,0);
set_motor_power(2,0);
set_motor_power(3,0);
compass_init();
sonar_init();
calCompass();
while(1)
{
clear_screen();
result = compass();
/* use calibration data */
result.x -= compZero.x;
result.y -= compZero.y;
print_string("x "); /* 2 */
print_int(result.x); /* 3 */
print_string(" y "); /* 3 */
print_int(result.y); /* 3 */
/* print_string(" s "); */ /* 3 */
/* print_int(getSonar(0)); */ /* 3 */
/* =17 */
next_line();
/* print_string("a "); */ /* 2 */
/* print_int(IR(0)); */ /* 3 */
/* print_string(" b "); */ /* 3 */
/* print_int(analog(1)); */ /* 3 */
/* print_string(" c "); */ /* 3 */
/* print_int(analog(2)); */ /* 3 */
/* print_int(i++); */ /* =17 */
/* print_int(distance(0));
print_string(" "); */
print_int(sizeof(int));
delay_ms(200);
/* print_int(i);
i=sweep(); */
}
}
void motor_init(void) {
//enable timer (set prescalar to /8)
cli();
TCCR1B |= _BV(CS11);
sbi(TIMSK,3); //enable output compare OC1B
set_motor_power(0,0);
set_motor_power(1,0);
set_motor_power(2,0);
set_motor_power(3,0);
sei();
}
void orientate()
{
//sensor left=0 right=1
//wheels left=1 right=0
int bool1=0;
int bool2=0;
set_motor_power(0, 100);
set_motor_power(1,100);
delay_ms(100);
while(1){
if(digital(1)==1)
{
set_motor_power(0,0);
delay_ms(100);
print_string("left det");
bool1++;
}
if(digital(0)==1)
{
set_motor_power(1,0);
delay_ms(100);
print_string("right det");
bool2++;
}
if(bool1>=1 && bool2>=1)
break;
}
set_motor_power(0,0);
set_motor_power(1,0);
delay_ms(100);
clear_screen();
print_string("done");
}
void calCompass()
{
int xmin, xmax;
int ymin, ymax;
struct heading temp;
u08 i=0;
temp = compass();
xmin = temp.x;
xmax = temp.x;
ymin = temp.y;
ymax = temp.y;
clear_screen();
print_string("Calibrating");
next_line();
print_string("Compass");
set_motor_power(0,25);
set_motor_power(1,-25);
for(i=0; i<255; i++)
{
clear_screen();
print_string("Calibrating");
next_line();
print_string("Compass");
temp = compass();
if(xmin > temp.x)
xmin = temp.x;
else if(xmax < temp.x)
xmax = temp.x;
if(ymin > temp.y)
ymin = temp.y;
else if(ymax < temp.y)
ymax = temp.y;
delay_ms(10);
}
set_motor_power(0,0);
set_motor_power(1,0);
compZero.x = (xmin+xmax)>>1;
compZero.y = (ymin+ymax)>>1;
clear_screen();
print_string("x: ");
print_int( (xmin+xmax)>>1 );
next_line();
print_string("y: ");
print_int( (ymin+ymax)>>1 );
while(!get_sw1());
/* test calibration results
* x: -3, y: 5
* x: -4, y: 7
* x: -4, y: 6
* x: -5, y: 6
* x: -8, y: 8
*/
/* i = sine[i];
i = sine[i];*/
}
int main(void)
{
initialize();
servo_init();
motor_init();
sonar_init();
set_servo_position(0, 120); //center the servo
print_string("Rodentia Demo");
next_line();
print_string("by Austin");
led_on();
delay_ms(100);
led_off();
delay_ms(100);
led_on();
delay_ms(100);
led_off();
//wait for a start signal
while(!get_sw1())
{
/*u08 temp;
clear_screen();
temp = analog(0);
print_int(temp);*/
delay_ms(50);
}
led_on();
while(1)
{ //find and point toward heading with minimum sonar reading
u08 heading = 0;
/*u16 minHeading = 0;
u16 left = 0;
u08 aLeft = 1;
u16 right = 0;
u08 aRight = 1;*/
u16 sonar = 0x0;
u16 temp = 0;
set_servo_position(0, 120); //center the servo
set_motor_power(0, MIN_POWER);
set_motor_power(1, -MIN_POWER);
while(heading < 170) //what's the magic number?
{
/*u08 tempL = analog(1);
u08 tempR = analog(2);
if(tempL < 25 && aLeft)
{
set_motor_power(0, 0);
left++;
aLeft = 0;
}
if(tempL > 180 && !aLeft)
{
set_motor_power(0, 0);
left++;
aLeft = 1;
}
if(tempR < 25 && aRight)
{
set_motor_power(1, 0);
right++;
aRight = 0;
}
if(tempR > 180 && !aRight)
{
set_motor_power(1, 0);
right++;
aRight = 1;
}
if(left > heading && right > heading)*/
{
heading++;
temp = getSonar(0);
if(IR(0) > 70)
{
temp = 0;
}
if(temp > sonar)
{
sonar = temp;
//minHeading = heading;
}
clear_screen();
print_string("here=");
print_int(temp);
print_string(" max=");
print_int(sonar);
/*next_line();
print_string("Hdng=");
print_int(heading);
print_string(" minH=");
print_int(minHeading);
if(left < right)
{ //left motor on
set_motor_power(0, MIN_POWER);
}
else if (right < left)
{ //right motor on
set_motor_power(1, -MIN_POWER);
}
else
{
set_motor_power(0, MIN_POWER);
set_motor_power(1, -MIN_POWER);
}*/
}
}
if(sonar > 140)
{ //set a reasonable maximum value for sonar
sonar = 140;
}
//turn back to minumum heading
temp = 0;
while( temp < sonar )
{
temp = getSonar(0) + 3;
if(IR(0) > 70)
{
temp = 0;
}
clear_screen();
print_string("here=");
print_int(temp);
print_string(" max=");
print_int(sonar);
}
/*set_motor_power(0, -MIN_POWER);
set_motor_power(1, MIN_POWER);
while(heading > minHeading)
{
u08 tempL = analog(1);
u08 tempR = analog(2);
if(tempL < 25 && aLeft)
{
set_motor_power(0, 0);
left--;
aLeft = 0;
}
if(tempL > 180 && !aLeft)
{
set_motor_power(0, 0);
left--;
aLeft = 1;
}
if(tempR < 25 && aRight)
{
set_motor_power(1, 0);
right--;
aRight = 0;
}
if(tempR > 180 && !aRight)
{
set_motor_power(1, 0);
right--;
aRight = 1;
}
if(left < heading && right < heading)
{
heading--;
clear_screen();
print_string("L=");
print_int(left);
print_string(" R=");
print_int(right);
next_line();
print_string("Hdng=");
print_int(heading);
print_string(" minH=");
print_int(minHeading);
if(left > right)
{ //left motor on
set_motor_power(0, -MIN_POWER);
}
else if (right > left)
{ //right motor on
set_motor_power(1, MIN_POWER);
}
else
{
set_motor_power(0, -MIN_POWER);
set_motor_power(1, MIN_POWER);
}
}
}*/
set_motor_power(0, 0);
set_motor_power(1, 0);
//scan with IR for obstacles
delay_ms(500);
u08 dir=120;
s08 dDir = 1;
u08 dist = IR(0);
set_motor_power(0, 100);
set_motor_power(1, 100);
while(dist < 100)
{
dir += dDir;
set_servo_position(0, dir);
if(dir >= 200)
{
dDir = -1;
}
if(dir <= 40)
{
dDir = 1;
}
dist = IR(0);
clear_screen();
print_string("Distance=");
print_int(dist);
}
}
/*u08 temp;
while(1)
{
clear_screen();
temp = analog(0);
print_int(temp);
set_servo_position(0,temp);
delay_ms(50);
}*/
}
void bt_packet_set_motor_power(bt_packet_t *p, uint8_t port, int power)
{
set_motor_power(&p->packets[0], port, power);
}
void stop()
{
set_motor_power(0, 0);
set_motor_power(1, 0);
}
