void ServoTestWizardPage::cleanupPage()
{
m_timer->stop();
servoOff(0);
servoOff(1);
servoOff(2);
servoOff(3);
}
void ServoTestWizardPage::initializePage()
{
servoOff(0);
servoOff(1);
servoOff(2);
servoOff(3);
m_timer->start(10);
}
void expand() {
servo(1, STL);
delayMs(100);
servo(5, STR);
delayMs(100);
/* servo(4, SBL);
delayMs(100);
servo(6, SBR);
delayMs(100);
*/
servoOff(1);
// servoOff(4);
servoOff(5);
// servoOff(6);
}
ServoTestWizardPage::ServoTestWizardPage(QWidget *parent)
: OnOffWizardPage(parent),
m_theta(0.0),
m_timer(new QTimer(this))
{
setTitle(tr("Servo Test"));
connect(this, SIGNAL(on(quint16)), SLOT(servoOn(quint16)));
connect(this, SIGNAL(off(quint16)), SLOT(servoOff(quint16)));
}
void find_first_well() {
//first we make sure we're on an open space
long open_space=0;//automatically intialized to zero
//printf("looking for a good start\n");
servoOn(1); //1 sets DIR to clockwise
do {
_delay_us(100);
if ( PIND & _BV(PD2) ) {
open_space += 100; //more open_space
// printf("%ld \n", open_space);
} else {
open_space = 0; //if no open space then reset counter
// printf("%ld \n", open_space);
}
} while (open_space < 400000L);
servoOff();
// printf("let us begin our adventure\n");
//_delay_ms(1000);
//begin first dot sequence
servoOn(1);
int counter = 0;
while (counter < 1000)
{
if ( PIND & _BV(PD2) ) {
_delay_us(10); // this is what happens if nothing detected. (detection events give low value)
counter = 0;
} else {
counter++;
//printf("counter is %d \n", counter); // don't use printf it slows everything down ruining timing measurement
}
}
servoOff();
// printf("what do we have here -- a well?\n");
//_delay_ms(1000);
go_to_next_dot(); //begin counting dots
}
void PololuServoController::disconnect() {
if (mConnected) {
reset(); //move to the default position
Sleep(100); //for the inertia!!!
for (int i=0; i<mNumServo; i++) {
servoOff(i); //turn off the servos
}
mPol.Close();
mConnected = false;
cout << "Polulo Controller Disconnected" << endl;
}
}
void go_to_next_edge(void) {
servoOn(1);
int counter = 0;
while (counter < 10)
{
if ( !(PIND & _BV(PD2)) ) {
_delay_ms(1); // this is what happens if nothing detected. (detection events give low value)
counter = 0;
} else {
counter++;
//printf("counter is %d \n", counter); // don't use printf it slows everything down ruining timing measurement
}
}
servoOff();
}
//This function initializes the timers so that the other servo
//routines function correctly. Call this function before
//using any of the servo functions.
//Normally called only by the initialize() function in utility.c.
void servoInit()
{
u08 i;
for (i = 0; i < NUM_SERVOS; i++)
{
servoOff(i);
}
//enable timer (set prescalar to /8)
TCCR3B |= _BV(CS31);
//enable interrupt for OCIE3C
TIMSK3 |= _BV(OCIE3C);
//enable interrupts
sei();
}
void go_to_next_dot() {
int count_dots=0;
int last_dot=0;
int there_is_a_dot=3;
while (last_dot==0) { //wait why 11? oh if misaligned and detects too many dots
//look for dot, but with a time limit
// printf("there is a dot preresult %d \n", there_is_a_dot);
servoOn(1);
there_is_a_dot = check_if_there_is_a_dot_there( 500000L ); //DONE -- replaced this with a check_if_there_is_a_dot_there function
//servoOff();
//_delay_ms(1000);
//printf("there is a dot result %d \n", there_is_a_dot);
//printf("how many dots %d \n", count_dots);
if (there_is_a_dot == 1)
{
count_dots++;
//printf("how many dots %d \n", count_dots);
//go to space right after the dot then begin the loop again
go_to_next_edge();
//printf("how many dots %d \n", count_dots);
} else if (there_is_a_dot==0) {
servoOff(); // stop
_delay_ms(100); //wait for stop
servoOn(0); // go reverse
//printf("is this the real mistake? %d\n", count_dots);
check_if_there_is_a_dot_there( 1000000L ); //check for the dot
servoOff(); // found the dot, next we will go a bit further to center pic
//_delay_ms(1000);
//printf("found the edge, hopefully not more dots %d\n", count_dots);
servoOn(0);
_delay_ms(600); // hopefully this is long enough
servoOff();
last_dot = 1; //break loop as we found last dot
} else {
//printf("count them dots %d\n", count_dots);
}
}
// send back the number of dots to pythonSerial.py
switch (count_dots) {
case 1:
printf("%d\r\n", 7);
break;
case 2:
printf("%d\r\n", 8);
break;
case 3:
printf("%d\r\n", 9);
break;
case 4:
printf("%d\r\n", 10);
break;
default:
break;
}
PORTB |= _BV(LED); // TODO Test LED ON
_delay_ms(2000); // wait for camera to take photo
PORTB &= ~_BV(LED); // TODO Test LED OFF
servoOn(1);
_delay_ms(1000);
count_dots = 0; // reset our global dot counter for next well
}
