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?
User | Revision | Line number | New 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 |