MiceBot
Leonardo Falcon
Robot Name
University of Florida
Department of Electrical and Computer Engineering
EEL4665 – IMDL
Instructors: A. Antonio Arroyo, Eric M. Schwartz
TAs: Josh Weaver, Ryan Chilton
2
Contents
Abstract---------------------------------------------------------------------------------------------------------3
Executive Summary-------------------------------------------------------------------------------------------3
Introduction----------------------------------------------------------------------------------------------------3
Integrated System---------------------------------------------------------------------------------------------3
Mobile Platform-----------------------------------------------------------------------------------------------4
Actuation-------------------------------------------------------------------------------------------------------5
Sensors----------------------------------------------------------------------------------------------------------5
Behaviors-------------------------------------------------------------------------------------------------------7
Experimental Layout and Results---------------------------------------------------------------------------7
Conclusion-----------------------------------------------------------------------------------------------------8
Documentation-------------------------------------------------------------------------------------------------9
Appendices-----------------------------------------------------------------------------------------------------9
3
Abstract
The Micebot is an autonomous robot that can travel to a specific relative location as
performing obstacle avoidance and remembering its trajectory. It is also able to complete shapes
(i.e. isosceles triangle). Two optical mouse sensors mounted to the bottom of the robot measure
distance with high accuracy, while range sensors detect obstacles in the direct path to the target.
Executive Summary
The Micebot is on a three wheel platform, actuated by two DC motors and balanced by a
caster wheel. It uses two infrared range sensors and a sonar range sensor to perform obstacle
detection and avoidance. The name Micebot originated from the robot using two optical mouse
sensors mounted to the bottom to keep track of the robot’s trajectory. The combination of these
sensors gives Micebot the ability to travel to a specific location even as obstacles stand it its way.
It uses trigonometry to keep differential values of the perpendicular distance to the straight line
path as well as the distance remaining to the target. After it gets around an obstacle it calculates
the angle to the target and proceeds in a straight line towards it, as searching for more obstacles.
When the final destination is reached, the robot can return to its starting position by retrieving
distances and angles from a memory array that recorded its trajectory. To further demonstrate the
capabilities of mouse sensors it also performs a calibration routine for adjusting to a new surface,
and it can move in the form of an isosceles triangle and return to its original position.
Introduction
The idea for the Micebot began as a curiosity for reaching a target via sound localization.
With localization comes navigation, and this quickly became the focus of this project. I looked
into the typical navigation sensors such as magnetometers, GPS, shaft encoders, and
accelerometers. However, the accuracy of these sensors was not ideal, within my budget. After
doing some research I found a few projects that involved interfacing optical mouse sensors with
microprocessors for various applications including the measurement of distance. The advantage
of this was that the reference point of these sensors was the surface and hence they measured the
actual movement. The next issue was in measuring rotation, this was solved using two mouse
sensors. Consequently, the goal of this project was to build an autonomous robot that could
display the capabilities of two optical mouse sensors as navigation tools. This was done by
programming a series of behaviors: Move in specific shape patterns, travel in a straight line,
remember trajectory for a return trip, and reach a specific relative location as performing
obstacle avoidance. This final behavior could have applications in mapping an inaccessible area,
and safely retrieving the robot after the mission. In this report the various aspects of this project
are outlined and reasoned.
Integrated System
A schematic of the integrated system is shown below. The Micebot is controlled by an
ATxmega128A1U microcontroller on an Epiphany DIY running at 32MHz with the Dual Motor
4
Driver package designed by Tim Martin. It is able to interface all the components needed for this
application. It includes a dual motor driver that is already connected to a PWM signal output
from the microprocessor; these control the DC motors powering the wheels. The board also
regulates an input between 9 and 14V down to 3.3V for powering the microprocessor. The
analog inputs from the IR range sensors and the Sonar sensors are fed through an ADC converter
in the microprocessor and processed to achieve obstacle avoidance. While, two optical mouse
sensors interfaced via two wire serial interface, keep a precise account of linear distance and in
place angular rotations. The board also has an XBee Radio Adapter that is already connected to
one of the UART modules of the Xmega, minimizing external wiring. An external 5V regulator
is used to supply the required 5V to the optical mouse sensors.
Mobile Platform
The robot is on a simple 3 wheel platform. Two
wheels provide actuation in a straight line and
differential steering, while a caster wheel maintains
balance. It was crucial that the robot rotate in place
as well as travel straight as possible at a constant
speed. To attain this goal, two mouse sensors were
placed beneath the robot and software maintains the
robot going straight and turning in place. Special
care was taken to make the set up as symmetrical as
possible, when mounting the motors and the mouse
sensors. This assures that data from the mouse
sensors is directly related to the actuation coming from the motors. Lastly, in order to achieve
Epiphany DIY with
ATxmega128A1U
Optical Mouse
Sensors (x2)
Motor Drivers
12 V supply
XCTU Terminal
Feedback &
Input
Sonar
IR Range
Finders (x2)
Bump Sensor
XBee
Figure 1: Platform without electronics.
5V regulator
5
proximity to the ground required by the mouse sensors, the motors were mounted on top of the
bottom level as shown in Figure 1.
Actuation Micebot is actuated by two 110RPM 12V Microgear DC motors from Servo
City. Their stall torque is 60oz-in. Two Pololu 60mm wheels are mounted
on to these motors.
Sensors
Sensors inform a robot of what is occurring in its environment, in
this case in order to travel effectively around obstacles, as well as track its
trajectory. The sonar used was the ultrasonic range finder EZ3 from Maxbotix. This was chosen
because of its thin 30cm cone when powered by 3.3V. This module can be read in a variety of
ways: Serial, PWM, and Analog. In this case the interface chosen was the analog signal
connected to an ADC pin on the Epiphany. Readings can occur at a 10Hz rate in the free running
mode of operation. Its resolution is 3mV/cm and can detect objects up to 600cm away.
Two IR range finders were placed at a 45 degree angle from the center of the robot on
both sides. The IR sensors used were the SHARP GP2Y0A21YK. Their valid range is from 10 –
80cm. Characterization experiments follow closely with the graph provided by the datasheet in
Figure 3.
Figure 3: Sonar range sensor Figure 4: IR range sensor
Figure 2: Microgear DC motor
Figure 5: IR Sensor Configuration
6
Navigation called for two mouse sensors side by side.
The mouse sensor chosen was a Parallax Mouse Sensor Kit which
is basically a breakout board for a MCS-12086 mouse sensor
chip. The way a mouse sensor works is a CMOS camera captures
images of a flat surface, brightened by an LED, and within a close
range ~2mm. These images are fed into a DSP that via a
mathematical algorithm compares them to determine a change in
x and y since the last reading. These values are put into registers
that are clocked in by the Epiphany DIY. The code to accomplish
this serial communication
can be found in the
appendices. The power
required for this module is 5V, and for snappier operation
the datasheets suggested that the power line be the same as
the data clock line. To clock a 5V signal with the 3.3V
Xmega required a simple external circuit provided in the
datasheet and shown in Figure 8. However, this circuit was
altered slightly so that both sensors could be clocked by the
same signal. The 5V were created with a voltage regulator
connected to the 12V power supply that powers the board
and the motors. The two mouse sensors were purposely
mounted at the bottom of the robot collinear with the
touching point of the wheels (Figure 6). The reason for this
setup is so that the center of rotation is at the midpoint
between the two wheels and angular measurements could be
made appropriately.
ddddddddddddd
Fgure 7: Parallax Mouse sensor
Figure 6: Strategic mounting of mouse sensors
Figure 8: Simple circuit for clocking a 5V signal with a 3.3V pin from the uP
7
Behaviors
Before the robot can begin rotating to specific angles and travelling specific distances, a
calibration process must take place, because mouse sensors react differently to different surfaces
depending on factors such as reflection. The rotation calibration was done by measuring a full
rotation triggered and ended by a thin stationary object placed at the left IR sensor. The distance
calibration was done by having the robot move in a straight line until the front bump switch is hit
at a distance of 1 foot. Finally, to end the calibration process the robot goes through a sequence
of movements to gather data on how much drift occurs on the specific surface during rotation
and straight maneuvers. Drift in this case is the inertial continuation of movement by the robot
after the microcontroller sends the stop command. After the calibration process is complete the
data is live fed to the Xbee terminal and can be hard coded to prevent recalibration in subsequent
runs after reset.
Now, the Micebot is ready to demonstrate precise movements. The coded behavior in this
case is a triangle. The robot i turn then it i tra e strai ht for feet turn ri ht
tra e strai ht feet turn ri ht , and travel straight 2.82 feet to complete the isosceles
triangle. Lastly it will rotate back to its starting position.
The final and most complex behavior involves the Micebot traversing a maze of obstacles
to reach a specific relative location. It uses its Sonar and IR sensors to get around obstacles while
tracking its trajectory with the mouse sensors. After each obstacle is cleared it will calculate the
turning angle towards the target and begin going straight as it searches for new obstacles. The
algorithm assumes the robot travels in a perfect straight line and rotates in place. After each turn
and straight line combination the microcontroller uses trigonometric functions to calculate an X
and Y differential value from the target. It uses these values and the arctangent function to
calculate the angle towards the target. When both X and Y differential values reach 0, the robot
has reached its final destination. The robot stores distances and angles in a memory array. This
allows it to return to its starting position repeating the exact movements made to get there.
Experimental layout and Results
For most debugging and experiments serial communication was used between two XBee
Series 1 modules. One connected to the Epiphany and the other to a USB connection on a laptop
running XCTU serial terminal. This was crucial because the Micebot had to be in motion during
most experiments, hence wireless communication was required.
The range sensors (Sonar & IR) were characterized by placing objects at predetermined
distances and reading the 12bit resolution values off of the XCTU serial terminal. The code used
for these experiments can be found in the appendices. These values were then taken down and
used in the code accordin to the desired thresho d for the robot’s obstac e a oidance beha ior.
The values from the Sonar sensor proved to be very sporadic at times, and with the
presence of outliers. To solve this issue a 32 iterations averaging loop was introduced and called
8
when needed for obstacle avoidance. This solved the issue of the robot reacting to obstacles that
did not exist.
The mouse sensors were also tested using the serial terminal. Firstly, to make sure the dx
and dy values were within - and meanin the re isters eren’t saturatin due to slow
clocking. At the Micebot’s top speed the delta registers had values from -15 - 15, meaning a
sufficient clocking speed. These delta values were then placed into cumulative variables. To
keep these from saturating they were placed in signed 32 bit variables and reset after each
maneuver.
The biggest experimental issue was drift, caused by the slight continuation of the motors
when commanded to stop after each maneuver, causing for the robot to think it was in a position
that it was not. This was solved by introducing a 300 ms. delay after each maneuver to allow for
the robot become completely stationary and then reading the optical mouse registers to account
for the drift. Knowing this drift is also important when rotating towards the target because the
drift value has to be discounted for more accuracy.
The code used for reaching a target as doing obstacle avoidance reached its final draft
after numerous debugging iterations. Before moving to multiple obstacles, tests were conducted
for successfully get around one obstacle and reach the target. During this stage of testing the
XCTU terminal was displaying specific distances travelled after each maneuver, the direction of
the next turn, the current angle position of the robot, as well as the distance towards the target,
and the sensor thresholds that the robot reacted to (Figure 9).
Figure 9: XCTU printout of a one obstacle run
Conclusions
There are a number of advantages to using optical mouse sensors as trajectory tracking
devices. Firstly, their point of reference is the surface, hence a more direct source of movement
than shaft encoders. Also, they yield 2 directional tracking which was very useful, as an
9
alternative to a Peripheral Integral Derivative (PID) controller, for going straight. However,
optical mouse sensors also have many disadvantages. Most importantly, they do not work well
on all surfaces. It struggles with highly reflective surfaces, due to its CMOS camera and bright
LED set up. It also requires high proximity to a flat surface for successful measurements.
Though very accurate after 10-15 maneuvers, it begins to accumulate significant error
after that. As this project continues after the end of this course, a secondary sensor will be
included to cross reference with the mouse sensors and improve accuracy. These secondary
sensors may include an IMU or another pair of mouse sensors. The latter can be seen done in a
project linked in the documentation, where four mouse sensors are used. Another addition I will
work on is adding a PID controller for greater straight line accuracy.
Documentation
Epiphany Manual from www.ootbrobotics.com
ATXmega128AU1 manual
Parallax MouseSensorKit v1.0
Sharp IR Sensor - GP2Y0A21YK
Sonar Sensor -MB1230-MB1330
Optical Mouse Sensor Chip - MCS-12086
LM2940 5V Voltage Regulator Datasheet.
XBee®/XBee‐PRO® RF Modules
https://instruct1.cit.cornell.edu/Courses/ee476/FinalProjects/s2009/ncr6_wjw27/ncr6_wj
w27/docs/dead_reckoning_with_mouse_sensors.pdf
Appendices
Atmel Studio Code
Final Code can be found at: https://sites.google.com/site/imdlmicebot/my-forms?pli=1
/*********************************************************************************
* Created: 1/31/2013 2:05:39 PM
* Author: Leonardo Falcon
This code prints the output of the Sonar sensor and an IR sensor simultaneously. Uses the libraries
provided by Tim Martin.
**********************************************************************************
*/
#include <avr/io.h>
#include <util/delay.h>
#include <math.h>
#include "clock.h"
#include "uart.h"
#include "adc.h"
#include <stdio.h>
10
int main(void)
{
clockInit();
adcInit(&ADCA);
usartInit(&USARTC0,57600);
sei();
PORTR.DIRSET = 0x02;
int count=0;
adcChannelMux(&ADCA,0,0);
adcChannelMux(&ADCA,1,2);
while(1)
{
count++;
PORTR_OUTTGL = 0x02;
_delay_ms(100);
fprintf(&USB_str,"Reading #%d: ", count);
fprintf(&USB_str,"IR: %d\t\t", analogRead(&ADCA,0));
fprintf(&USB_str,"Sonar: %d\r\n", analogRead(&ADCA,1));
}
}
/************************************************************************************
* Created: 1/31/2013 2:05:39 PM
* Author: Leonardo Falcon
* This is an obstacle avoidance program. When one IR sensor comes within the threshold it will turn
away from that obstacle. When the sonar sensor comes within the threshold it will rotate in the direction
of the IR sensor with the smallest value until the Sonar sensor senses a second threshold voltage
indicating a clear path. This prevents being trapped in corners.
*************************************************************************************
*/
#include <avr/io.h>
#include <util/delay.h>
#include <math.h>
#include "clock.h"
#include "uart.h"
#include "adc.h"
#include "motor.h"
#include <stdio.h>
11
int main(void)
{
clockInit();
adcInit(&ADCA);
usartInit(&USARTC0,57600);
motorInit();
sei();
PORTR.DIRSET = 0x02;
int count=0;
adcChannelMux(&ADCA,0,0);
adcChannelMux(&ADCA,1,2);
adcChannelMux(&ADCA,2,4);
while(count<50) //5 second delay to set robot in place
{
count++;
PORTR_OUTTGL = 0x02;
_delay_ms(100);
}
while(1)
{
if(analogRead (&ADCA,0)<280)
{
if(analogRead(&ADCA,2)>analogRead(&ADCA,1))
{
while(analogRead(&ADCA,0) < 300)
{
setMotorEffort(1,900,MOTOR_DIR_BACKWARD);
setMotorEffort(2,900,MOTOR_DIR_FORWARD);
_delay_ms(100);
}
}
else
{
while(analogRead(&ADCA,0) < 300)
{
setMotorEffort(2,900,MOTOR_DIR_BACKWARD);
setMotorEffort(1,900,MOTOR_DIR_FORWARD);
_delay_ms(100);
}
}
}
12
else if(analogRead(&ADCA,1)>1500)
{
setMotorEffort(1,900,MOTOR_DIR_FORWARD);
setMotorEffort(2,900,MOTOR_DIR_BACKWARD);
}
else if(analogRead (&ADCA,2)>1500)
{
setMotorEffort(1,900,MOTOR_DIR_BACKWARD);
setMotorEffort(2,900,MOTOR_DIR_FORWARD);
}
else
{
setMotorEffort(1,900,MOTOR_DIR_FORWARD);
setMotorEffort(2,900,MOTOR_DIR_FORWARD);
}
// fprintf(&USB_str,"Reading #%d: \t\t", count);
// fprintf(&USB_str,"IR: %d\t\t", analogRead(&ADCA,1));
// fprintf(&USB_str,"Sonar: %d\t\t", analogRead(&ADCA,0));
// fprintf(&USB_str,"IR2 %d\r\n", analogRead(&ADCA,2));
_delay_ms(100);
}
}
/************************************************************************************
* Created: 3/10/2013 2:05:39 PM
* Author: Leonardo Falcon
* ParallaxMouseSensor.c
* This is source code developed for clocking data in and out of the x and y registers in the mouse sensor.
This code was adapted from .BS2 sample code provided in the datasheet of the sensors as well as close
analysis of the MCS-12086 sensor chip datasheet. This code will only work if the mouse sensors are
powered from the clock line.
*************************************************************************************
*/
#include <ParallaxMouseSensor.h> #include <util/delay.h> #include <uart.h> //Mouse sensor register addresses #define CONF 0x1B #define LORES 0x80 #define CHNG 0x80 #define OFLOW 0x18 #define NEG 0x80
13
void mouseConfigure() { y_left=y_right=0; x_left=x_right=0; PORTE.DIRSET = 0x1; //sets pin 0 in port E as CLOCK PORTE.OUTCLR = 0x1; _delay_ms(300); PORTE.OUTSET = 0x1; //sets CLOCK HIGH _delay_ms(100); //Wait for mouse sensor to come out of RESET mouseWriteAddr(CONF, LORES); } void mouseWriteAddr(int8_t address, int8_t data) { PORTD.DIRSET = 0xC0; //sets pin 6 & 7 as output DATA PORTE.DIRSET = 0x1; //sets pin 6 as CLOCK PORTE.OUTSET = 0x1; //sets CLOCK HIGH address = address|0x80; for(int i=0; i<=7; i++) { PORTE.OUTSET = 0x1; PORTD.OUT = (address & 0x80) | (((address & 0x80) >> 1) & 0x40); //sends out the same bit to mouse sensor 1 and 2 through a masking equation _delay_us(2); PORTE.OUTCLR = 0x1; _delay_us(2); address = address << 1; } _delay_us(100); for(int i=0; i<=7; i++) { PORTE.OUTSET = 0x1; PORTD.OUT = (data & 0x80) | (((data & 0x80) >> 1) & 0x40); _delay_us(2); PORTE.OUTCLR = 0x1; _delay_us(2); data = data << 1; } PORTE.OUTSET= 0x1; //sets CLOCK/POWER back to HIGH _delay_us(100); } void mouseReadAddr(int8_t address) { data_left=0; data_right=0; int8_t temp; PORTD.DIRSET = 0xC0; //sets pin 6 & 7 as output DATA PORTE.DIRSET = 0x1; //sets pin 0 as CLOCK for(int i=0; i<=7; i++) { PORTE.OUTSET = 0x1; PORTD.OUT = (address & 0x80) | (((address & 0x80) >> 1) & 0x40); //sends out the same bit to mouse sensor 1 and 2 _delay_us(2);
14
PORTE.OUTCLR = 0x1; _delay_us(2); address = address << 1; } PORTE.OUTSET = 0x1; _delay_us(100); PORTD.DIRCLR = 0xC0; //sets pin 6 & 7 as input DATA for (int i=0; i<=7; i++) { data_left = data_left << 1; data_right = data_right << 1; PORTE.OUTCLR = 0x01; _delay_us(2); temp = PORTD.IN; temp = temp >> 6; data_right = data_right | (temp & 0x01); temp = temp >> 1; data_left = data_left | (temp & 0x01); PORTE.OUTSET = 0x1; _delay_us(2); } _delay_us(100); } void mouseReadSensor() { mouseReadAddr(0x16); //checks if any movement has occurred. stat_left = data_left; stat_right = data_right; stat = stat_right | stat_left; if((stat & CHNG) == 0) { } else { if(stat & OFLOW) { ovfl = 10; } mouseReadAddr(0x03); readx_left = data_left; readx_right = data_right; x_right = x_right + readx_right; x_left = x_left + readx_left; // fprintf (&Xbee_str, "%d \t\t", x_left); // fprintf (&Xbee_str, "%d \t\t", x_right); ready_left = 0; //reset deltas ready_right = 0; mouseReadAddr(0x02); ready_left = data_left; ready_right = data_right; y_right = y_right + ready_right; y_left = y_left + ready_left; // fprintf (&Xbee_str, "%d \t\t", ready_left);
15
// fprintf (&Xbee_str, "%d \t\t", ready_left); // fprintf (&Xbee_str, "%d \t\t", y_left); // fprintf (&Xbee_str, "%d \r\n", y_right); if(ovfl > 0) { ovfl = ovfl - 1; } } }
/************************************************************************************
* Created: 3/10/2013 2:05:39 PM
* Author: Leonardo Falcon
* ParallaxMouseSensor.h
* Header file created to call mouse sensor data functions from main.
*************************************************************************************
*/
#ifndef ParallaxMouseSensor_H_ #define ParallaxMouseSensor_H_ #include <avr/io.h> void mouseConfigure(); void mouseReadSensor(); void mouseWriteAddr(int8_t address, int8_t data); void mouseReadAddr(int8_t address); int x_right; int x_left; int64_t y_right; int64_t y_left; int8_t quality; int8_t ovfl; int8_t stat_right; int8_t stat_left; int8_t stat; int8_t readx_left; int8_t readx_right; int8_t ready_left; int8_t ready_right; int8_t data_left; int8_t data_right; #endif
/************************************************************************************
* Created: 3/25/2013 2:05:39 AM
* Author: Leonardo Falcon
16
* ParallaxMouseSensor.c
* Calibration code: This is an excerpt from the final code, it informs the robot of the mouse sensor
readings that represent 1 foot in a straight line, and a 360 degree angle. It also, measures and stores drift
values.
*************************************************************************************
*/
mouseConfigure(); //resets mouse sensor registers, and y_left and y_right fprintf(&Xbee_str, "Distance Calibration for 1 foot..\r\n"); PORTC.DIRCLR=0x80; while ((PORTC.IN & 0x80) == 0) { GoStraight(); mouseReadSensor(); } footdistance= (y_left + y_right)/2; Stop(); PORTR.OUTTGL=0x02; fprintf(&Xbee_str, "1 foot= %d\r\n", (y_left + y_right)/2); fprintf(&Xbee_str, "Hold down bump switch and choose number of feet \r\n"); _delay_ms(3000); while (PORTC.IN & 0x80); totdistance = footdistance * (command & 0xF); distance2goY= totdistance; fprintf(&Xbee_str,"Distance to go: %d\r\n", distance2goY); _delay_ms(3000); mouseConfigure(); count = 0; while (count< 0xCFF) { GoStraight(); count++; mouseReadSensor(); } Stop(); straightdrift= (y_left + y_right)/2; count = 0; while (count< 0x7FF) { count++; mouseReadSensor(); } straightdrift = (y_left + y_right)/2 - straightdrift; fprintf(&Xbee_str, "Straight line drift: %d\r\n\r\n", straightdrift); } //Straight line calibration completed. fprintf(&Xbee_str, "Beginning rotation calibration...\r\n"); while (analogRead(&ADCA,1) < 1980); count = 0; mouseConfigure(); RotateCounterClock(); while (count < 0x3FF) {
17
count++; mouseReadSensor(); } while (analogRead(&ADCA,1) < 1980) { mouseReadSensor(); } fullrot=y_right-y_left; Stop(); while (count< 0x7FF) { count++; mouseReadSensor(); } drift = (y_right - y_left) - fullrot; fprintf (&Xbee_str, "drift: %d\t\t", drift); factor360= 2 * M_PI / fullrot; fprintf(&Xbee_str, "Calibration Completed.. Full Rotation Value: %d\r\n", fullrot);
/************************************************************************************
* Created: 3/30/2013 2:05:39 AM
* Author: Leonardo Falcon
* ParallaxMouseSensor.c
* This code is an excerpt from the final code. It takes the robot on a isosceles triangle trajectory and
returns to the exact starting position.
*************************************************************************************
*/
mouseConfigure(); RotateClock(); while ((y_left-y_right+drift)*factor360 < M_PI_4) { mouseReadSensor(); } Stop(); _delay_ms(300); mouseConfigure(); while ((y_right+y_left+straightdrift)/2 < 2*footdistance) { GoStraight(); mouseReadSensor(); } Stop(); _delay_ms(300); mouseConfigure(); RotateClock(); while ((y_left-y_right+drift)*factor360 < M_PI_2) { mouseReadSensor(); } Stop(); _delay_ms(300); MouseConfigure(); while ((y_right+y_left+straightdrift)/2 < 2*footdistance) {
18
GoStraight(); mouseReadSensor(); } Stop(); _delay_ms(300); mouseConfigure(); RotateClock(); while ((y_left-y_right+drift)*factor360 < 3*M_PI_4) { mouseReadSensor(); } Stop(); _delay_ms(300); mouseConfigure(); while ((y_right+y_left+straightdrift)/2 < 2.82843*footdistance) { GoStraight(); mouseReadSensor(); } Stop(); _delay_ms(300); mouseConfigure(); RotateClock(); while ((y_left-y_right+drift)*factor360 < M_PI_2) { mouseReadSensor(); } Stop(); }
