Motor_encoder
Dependencies: mbed QEI Motordriver ros_lib_kinetic
main.cpp@0:3e435f58182d, 2019-11-08 (annotated)
- Committer:
- dnulty
- Date:
- Fri Nov 08 10:35:42 2019 +0000
- Revision:
- 0:3e435f58182d
Encoder
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dnulty | 0:3e435f58182d | 1 | /* Maths behind the Encoder |
dnulty | 0:3e435f58182d | 2 | 12 readings per revolution, 298:1 gear ratio. 3576 pulses per revoltion |
dnulty | 0:3e435f58182d | 3 | |
dnulty | 0:3e435f58182d | 4 | Note distances are based of wheel spec and so are approximations |
dnulty | 0:3e435f58182d | 5 | Diameter of wheel = 60mm, circumference = 188.5 mm, distance per revolution and second is 188.5mm |
dnulty | 0:3e435f58182d | 6 | Pulses to travel 30cm = 5691 |
dnulty | 0:3e435f58182d | 7 | Pulses to rotate 90 = 2700 |
dnulty | 0:3e435f58182d | 8 | */ |
dnulty | 0:3e435f58182d | 9 | |
dnulty | 0:3e435f58182d | 10 | #include "mbed.h" |
dnulty | 0:3e435f58182d | 11 | #include <motordriver.h> |
dnulty | 0:3e435f58182d | 12 | #include "QEI.h" |
dnulty | 0:3e435f58182d | 13 | #include <ros.h> |
dnulty | 0:3e435f58182d | 14 | #include <std_msgs/Int32.h> |
dnulty | 0:3e435f58182d | 15 | |
dnulty | 0:3e435f58182d | 16 | //Functions |
dnulty | 0:3e435f58182d | 17 | void move(float motorA, float motorB, int distance, int pulse_direction); |
dnulty | 0:3e435f58182d | 18 | |
dnulty | 0:3e435f58182d | 19 | //set up serial port |
dnulty | 0:3e435f58182d | 20 | Serial pc(USBTX, USBRX); |
dnulty | 0:3e435f58182d | 21 | |
dnulty | 0:3e435f58182d | 22 | // class set up |
dnulty | 0:3e435f58182d | 23 | class action { |
dnulty | 0:3e435f58182d | 24 | public: |
dnulty | 0:3e435f58182d | 25 | float motorA; |
dnulty | 0:3e435f58182d | 26 | float motorB; |
dnulty | 0:3e435f58182d | 27 | int distance ; |
dnulty | 0:3e435f58182d | 28 | int pulse_direction; |
dnulty | 0:3e435f58182d | 29 | }; |
dnulty | 0:3e435f58182d | 30 | |
dnulty | 0:3e435f58182d | 31 | // Set motorpins for driving |
dnulty | 0:3e435f58182d | 32 | Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake |
dnulty | 0:3e435f58182d | 33 | Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake |
dnulty | 0:3e435f58182d | 34 | QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding |
dnulty | 0:3e435f58182d | 35 | QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding |
dnulty | 0:3e435f58182d | 36 | |
dnulty | 0:3e435f58182d | 37 | int main(){ |
dnulty | 0:3e435f58182d | 38 | |
dnulty | 0:3e435f58182d | 39 | action forward{-0.5,-0.5,5691,1}; // class creation ..... edit action forward{,,This,} to change distance or rotation length on each |
dnulty | 0:3e435f58182d | 40 | move(forward.motorA, forward.motorB, forward.distance, forward.pulse_direction); // make the robot move |
dnulty | 0:3e435f58182d | 41 | |
dnulty | 0:3e435f58182d | 42 | action reverse{0.5,0.5,5691,-1}; |
dnulty | 0:3e435f58182d | 43 | move(reverse.motorA, reverse.motorB, reverse.distance, reverse.pulse_direction); |
dnulty | 0:3e435f58182d | 44 | |
dnulty | 0:3e435f58182d | 45 | action left{-0.5,0.5,3000,-1}; |
dnulty | 0:3e435f58182d | 46 | move(left.motorA, left.motorB, left.distance, left.pulse_direction); |
dnulty | 0:3e435f58182d | 47 | |
dnulty | 0:3e435f58182d | 48 | action right{0.5,-0.5,3000,1}; |
dnulty | 0:3e435f58182d | 49 | move(right.motorA, right.motorB, right.distance, right.pulse_direction); |
dnulty | 0:3e435f58182d | 50 | |
dnulty | 0:3e435f58182d | 51 | } |
dnulty | 0:3e435f58182d | 52 | |
dnulty | 0:3e435f58182d | 53 | |
dnulty | 0:3e435f58182d | 54 | |
dnulty | 0:3e435f58182d | 55 | void move(float motorA, float motorB, int distance, int pulse_direction){ |
dnulty | 0:3e435f58182d | 56 | //2 variables to allow for any pulse reading |
dnulty | 0:3e435f58182d | 57 | float initial_pulse_reading = wheel_B.getPulses(); |
dnulty | 0:3e435f58182d | 58 | float current_pulse_reading = wheel_B.getPulses(); |
dnulty | 0:3e435f58182d | 59 | // - is forward on our robot |
dnulty | 0:3e435f58182d | 60 | A.speed(motorA); |
dnulty | 0:3e435f58182d | 61 | B.speed(motorB); |
dnulty | 0:3e435f58182d | 62 | // loop for moving forward 30cm |
dnulty | 0:3e435f58182d | 63 | |
dnulty | 0:3e435f58182d | 64 | if (pulse_direction > 0){ |
dnulty | 0:3e435f58182d | 65 | while(current_pulse_reading <= (initial_pulse_reading + distance*pulse_direction)){ |
dnulty | 0:3e435f58182d | 66 | current_pulse_reading = wheel_B.getPulses(); |
dnulty | 0:3e435f58182d | 67 | } |
dnulty | 0:3e435f58182d | 68 | } |
dnulty | 0:3e435f58182d | 69 | else{ |
dnulty | 0:3e435f58182d | 70 | while(current_pulse_reading >= (initial_pulse_reading + distance*pulse_direction)){ |
dnulty | 0:3e435f58182d | 71 | current_pulse_reading = wheel_B.getPulses(); |
dnulty | 0:3e435f58182d | 72 | } |
dnulty | 0:3e435f58182d | 73 | } |
dnulty | 0:3e435f58182d | 74 | A.speed(0); |
dnulty | 0:3e435f58182d | 75 | B.speed(0); |
dnulty | 0:3e435f58182d | 76 | } |
dnulty | 0:3e435f58182d | 77 |