main.cpp

00001 /**
00002  * Drive a robot forwards or backwards by supplying a PWM signal to
00003  * H-bridges connected to the motors.
00004  */
00005 
00006 #include "Motor.h"
00007 #include "QEI.h"
00008 
00009 #define PULSES_PER_REVOLUTION 624
00010 
00011 Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
00012 Motor rightMotor(p22, p17, p18); //pwm, inA, inB
00013 QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
00014 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
00015 
00016 int main() {
00017     
00018     leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
00019     rightMotor.period(0.00005);
00020 
00021     int leftPulses  = 0;    //How far the left wheel has travelled.
00022     int rightPulses = 0;    //How far the right wheel has travelled.
00023     int distance    = 6000; //Number of pulses to travel.
00024 
00025     wait(5); //Wait a few seconds before we start moving.
00026     
00027     leftMotor.speed(0.4);
00028     rightMotor.speed(0.4);
00029     
00030     while((leftPulses < distance) || (rightPulses < distance)){
00031     
00032         leftPulses  = leftQei.getPulses();
00033         rightPulses = rightQei.getPulses();
00034         
00035     }
00036     
00037     leftMotor.brake();
00038     rightMotor.brake();
00039 
00040 }