K K / Mbed 2 deprecated experiment_program

The calibration code for our robot project

Dependencies:   MODSERIAL QEI mbed

main.cpp@0:8c140b332a82, 2015-09-28 (annotated)

Committer:
Technical_Muffin
Date:
Mon Sep 28 14:51:04 2015 +0000
Revision:
0:8c140b332a82
The calibration program for our robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Technical_Muffin 0:8c140b332a82 1 #include "mbed.h"
Technical_Muffin 0:8c140b332a82 2 #include "QEI.h"
Technical_Muffin 0:8c140b332a82 3 #include "MODSERIAL.h"
Technical_Muffin 0:8c140b332a82 4 //#include <"math.h">
Technical_Muffin 0:8c140b332a82 5
Technical_Muffin 0:8c140b332a82 6 QEI motor1(D13,D12,NC, 624);
Technical_Muffin 0:8c140b332a82 7 MODSERIAL pc(USBTX,USBRX);
Technical_Muffin 0:8c140b332a82 8 DigitalOut direction1(D7);
Technical_Muffin 0:8c140b332a82 9 PwmOut speed1(D6);
Technical_Muffin 0:8c140b332a82 10 DigitalIn button1(PTC6);
Technical_Muffin 0:8c140b332a82 11
Technical_Muffin 0:8c140b332a82 12 int main()
Technical_Muffin 0:8c140b332a82 13 {
Technical_Muffin 0:8c140b332a82 14 DigitalOut led(LED_RED);
Technical_Muffin 0:8c140b332a82 15 pc.baud(115200);
Technical_Muffin 0:8c140b332a82 16 int activation=0;//int for starting calibration
Technical_Muffin 0:8c140b332a82 17 float cycle = 0.6;//define the speed of the motor
Technical_Muffin 0:8c140b332a82 18 int first = 0;
Technical_Muffin 0:8c140b332a82 19 while(1){
Technical_Muffin 0:8c140b332a82 20 if (button1.read()==0){
Technical_Muffin 0:8c140b332a82 21 activation = 1;//acivate the int if the button is pressed
Technical_Muffin 0:8c140b332a82 22 }
Technical_Muffin 0:8c140b332a82 23
Technical_Muffin 0:8c140b332a82 24 //pc.printf("%d\n",activation);//test if the button is activated
Technical_Muffin 0:8c140b332a82 25 //wait(1);
Technical_Muffin 0:8c140b332a82 26
Technical_Muffin 0:8c140b332a82 27 if(activation == 1){
Technical_Muffin 0:8c140b332a82 28 DigitalOut led(LED_BLUE);
Technical_Muffin 0:8c140b332a82 29 direction1.write(false);//turn motor CCW or CW if set to 0
Technical_Muffin 0:8c140b332a82 30 //motor CW = 0
Technical_Muffin 0:8c140b332a82 31 //motor CCW = 1
Technical_Muffin 0:8c140b332a82 32 speed1.period_us(100);//Set period of PWM to 100 us.
Technical_Muffin 0:8c140b332a82 33 speed1.write(cycle);//write the speed to the motor
Technical_Muffin 0:8c140b332a82 34
Technical_Muffin 0:8c140b332a82 35
Technical_Muffin 0:8c140b332a82 36 if(first==0){//on the first loop wait a moment for the motor to start.
Technical_Muffin 0:8c140b332a82 37 wait(3);
Technical_Muffin 0:8c140b332a82 38 first=1;
Technical_Muffin 0:8c140b332a82 39 }
Technical_Muffin 0:8c140b332a82 40 //set up the speed calculation of the motor
Technical_Muffin 0:8c140b332a82 41 wait(0.1);
Technical_Muffin 0:8c140b332a82 42 int pos1=motor1.getPulses();
Technical_Muffin 0:8c140b332a82 43 //pc.printf("position 1 is: %d\n",pos1);//print position 1
Technical_Muffin 0:8c140b332a82 44 wait(0.1);
Technical_Muffin 0:8c140b332a82 45 int pos2=motor1.getPulses();
Technical_Muffin 0:8c140b332a82 46 //pc.printf("position 2 is: %d\n",pos2);//print position 1
Technical_Muffin 0:8c140b332a82 47 int diff = pos2 - pos1;//check if the encoder count stops changing
Technical_Muffin 0:8c140b332a82 48 int rads = abs(diff/0.1);
Technical_Muffin 0:8c140b332a82 49 pc.printf("the speed is : %d counts/second\n", rads);//print out the speed
Technical_Muffin 0:8c140b332a82 50 if(diff == 0){
Technical_Muffin 0:8c140b332a82 51 cycle=0;//speed of 0 to stop motor
Technical_Muffin 0:8c140b332a82 52 speed1.write(cycle);//write the new speed to the motor
Technical_Muffin 0:8c140b332a82 53 motor1.reset();//reset the motor position
Technical_Muffin 0:8c140b332a82 54 int posnew=motor1.getPulses();
Technical_Muffin 0:8c140b332a82 55 pc.printf("The new position is: %d\n",posnew);
Technical_Muffin 0:8c140b332a82 56 activation=0;
Technical_Muffin 0:8c140b332a82 57 break;
Technical_Muffin 0:8c140b332a82 58 }
Technical_Muffin 0:8c140b332a82 59 }
Technical_Muffin 0:8c140b332a82 60 }
Technical_Muffin 0:8c140b332a82 61 }
Technical_Muffin 0:8c140b332a82 62 //determine speed of movement from the encoder signal
Technical_Muffin 0:8c140b332a82 63 // the amount of counts for one revolution is 32
Technical_Muffin 0:8c140b332a82 64 //this is X2 encodng, as the QEI uses X2 by default
Technical_Muffin 0:8c140b332a82 65 //and the motor encoder has a X4 encoder and thus 64 counts per revolution
Technical_Muffin 0:8c140b332a82 66 //thus keeping in mind the gearbox with 1:131,25 gear ratio:
Technical_Muffin 0:8c140b332a82 67 // the amount of counts for a gearbox revolution or resolution is 8400
Technical_Muffin 0:8c140b332a82 68 // a revolution is 2 pi rad, resolution is : 8400/2 pi = 1336.90 counts/rad
Technical_Muffin 0:8c140b332a82 69 //or for the encoder x2 its 668,45 counts/rad
Technical_Muffin 0:8c140b332a82 70 /*
Technical_Muffin 0:8c140b332a82 71 Rotational position in degrees can be calculated by:
Technical_Muffin 0:8c140b332a82 72 *
Technical_Muffin 0:8c140b332a82 73 * (pulse count / X * N) * 360
Technical_Muffin 0:8c140b332a82 74 *
Technical_Muffin 0:8c140b332a82 75 * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
Technical_Muffin 0:8c140b332a82 76 * of pulses per revolution.
Technical_Muffin 0:8c140b332a82 77 *
Technical_Muffin 0:8c140b332a82 78 * Linear position can be calculated by:
Technical_Muffin 0:8c140b332a82 79 *
Technical_Muffin 0:8c140b332a82 80 * (pulse count / X * N) * (1 / PPI)
Technical_Muffin 0:8c140b332a82 81 *
Technical_Muffin 0:8c140b332a82 82 * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
Technical_Muffin 0:8c140b332a82 83 * pulses per revolution, and PPI is pulses per inch, or the equivalent for
Technical_Muffin 0:8c140b332a82 84 * any other unit of displacement. PPI can be calculated by taking the
Technical_Muffin 0:8c140b332a82 85 * circumference of the wheel or encoder disk and dividing it by the number
Technical_Muffin 0:8c140b332a82 86 * of pulses per revolution.
Technical_Muffin 0:8c140b332a82 87
Technical_Muffin 0:8c140b332a82 88 * Reset the encoder.
Technical_Muffin 0:8c140b332a82 89 *
Technical_Muffin 0:8c140b332a82 90 * Sets the pulses and revolutions count to zero.
Technical_Muffin 0:8c140b332a82 91
Technical_Muffin 0:8c140b332a82 92 void reset(void);
Technical_Muffin 0:8c140b332a82 93 */
Technical_Muffin 0:8c140b332a82 94