Soft robot team / Mbed 2 deprecated Motor_encoder

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?

UserRevisionLine numberNew 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