The calibration code for our robot project

Dependencies:   MODSERIAL QEI mbed

main.cpp

Committer:
Technical_Muffin
Date:
2015-09-28
Revision:
0:8c140b332a82

File content as of revision 0:8c140b332a82:

#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"
//#include <"math.h">

QEI motor1(D13,D12,NC, 624);
MODSERIAL pc(USBTX,USBRX);
DigitalOut direction1(D7);
PwmOut speed1(D6);
DigitalIn button1(PTC6);

int main()
{
   DigitalOut led(LED_RED);
   pc.baud(115200);
   int activation=0;//int for starting calibration
   float cycle = 0.6;//define the speed of the motor
   int first = 0;
   while(1){
       if (button1.read()==0){
       activation = 1;//acivate the int if the button is pressed
       }
    
    //pc.printf("%d\n",activation);//test if the button is activated
    //wait(1);
    
   if(activation == 1){
       DigitalOut led(LED_BLUE);
       direction1.write(false);//turn motor CCW or CW if set to 0
   //motor CW = 0
   //motor CCW = 1
       speed1.period_us(100);//Set period of PWM to 100 us.
       speed1.write(cycle);//write the speed to the motor
            
       
       if(first==0){//on the first loop wait a moment for the motor to start.
           wait(3);
           first=1;
           }
           //set up the speed calculation of the motor
       wait(0.1);
       int pos1=motor1.getPulses();
       //pc.printf("position 1 is: %d\n",pos1);//print position 1
       wait(0.1);
       int pos2=motor1.getPulses();
       //pc.printf("position 2 is: %d\n",pos2);//print position 1
       int diff = pos2 - pos1;//check if the encoder count stops changing
       int rads = abs(diff/0.1);
       pc.printf("the speed is : %d counts/second\n", rads);//print out the speed
       if(diff == 0){
           cycle=0;//speed of 0 to stop motor
           speed1.write(cycle);//write the new speed to the motor
           motor1.reset();//reset the motor position
           int posnew=motor1.getPulses();
           pc.printf("The new position is: %d\n",posnew);
           activation=0;
           break;
       }
       }
}
}
       //determine speed of movement from the encoder signal
       // the amount of counts for one revolution is 32
       //this is X2 encodng, as the QEI uses X2 by default 
       //and the motor encoder has a X4 encoder and thus 64 counts per revolution
       //thus keeping in mind the gearbox with 1:131,25 gear ratio:
       // the amount of counts for a gearbox revolution or resolution is 8400
       // a revolution is 2 pi rad, resolution is : 8400/2 pi = 1336.90 counts/rad
       //or for the encoder x2 its 668,45 counts/rad
 /*      
   Rotational position in degrees can be calculated by:
 *
 * (pulse count / X * N) * 360
 *
 * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
 * of pulses per revolution.
 *
 * Linear position can be calculated by:
 *
 * (pulse count / X * N) * (1 / PPI)
 *
 * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
 * pulses per revolution, and PPI is pulses per inch, or the equivalent for
 * any other unit of displacement. PPI can be calculated by taking the
 * circumference of the wheel or encoder disk and dividing it by the number
 * of pulses per revolution.
 
     * Reset the encoder.
     *
     * Sets the pulses and revolutions count to zero.
     
    void reset(void);
 */