code for basic movement of robot

Dependencies:   MODSERIAL QEI mbed

main.cpp

Committer:
Technical_Muffin
Date:
2015-10-07
Revision:
4:60bc2ee4d838
Parent:
3:b913f8ea69e8
Child:
5:a1376ab695f3

File content as of revision 4:60bc2ee4d838:

#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);
DigitalOut led1(LED_RED);
DigitalOut led2(LED_BLUE);
DigitalOut led3(LED_GREEN);

int main()
{
   float cycle = 0.6;//define the speed of the motor
   bool motor1_on = 1;
   int motor1_dir=0;
   
while(1){
       led3.write(0);
       led1.write(1);
       led2.write(1);
       speed1.write(0);
   pc.baud(115200);
   
   while(button1.read() != motor1_on){// turn on motor 1 when the button is being pressed
       led3.write(1);
       led1.write(0);
       
       direction1.write(motor1_dir);//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  
}
}
}
       //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);
 */