Soft robot team
Motor_encoder
Dependencies: mbed QEI Motordriver ros_lib_kinetic
Revision 0:3e435f58182d, committed 2019-11-08
- Comitter:
- dnulty
- Date:
- Fri Nov 08 10:35:42 2019 +0000
- Commit message:
- Encoder
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Fri Nov 08 10:35:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/farbodjam/code/Motordriver/#541fb1742c8b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Nov 08 10:35:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 08 10:35:42 2019 +0000
@@ -0,0 +1,77 @@
+/* Maths behind the Encoder
+ 12 readings per revolution, 298:1 gear ratio. 3576 pulses per revoltion
+
+ Note distances are based of wheel spec and so are approximations
+ Diameter of wheel = 60mm, circumference = 188.5 mm, distance per revolution and second is 188.5mm
+ Pulses to travel 30cm = 5691
+ Pulses to rotate 90 = 2700
+*/
+
+#include "mbed.h"
+#include <motordriver.h>
+#include "QEI.h"
+#include <ros.h>
+#include <std_msgs/Int32.h>
+
+//Functions
+void move(float motorA, float motorB, int distance, int pulse_direction);
+
+//set up serial port
+Serial pc(USBTX, USBRX);
+
+// class set up
+class action {
+ public:
+ float motorA;
+ float motorB;
+ int distance ;
+ int pulse_direction;
+};
+
+// Set motorpins for driving
+Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
+Motor B(PB_4, PC_7, PA_15, 1); // pwm, fwd, rev, can brake
+QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
+QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
+
+int main(){
+
+ action forward{-0.5,-0.5,5691,1}; // class creation ..... edit action forward{,,This,} to change distance or rotation length on each
+ move(forward.motorA, forward.motorB, forward.distance, forward.pulse_direction); // make the robot move
+
+ action reverse{0.5,0.5,5691,-1};
+ move(reverse.motorA, reverse.motorB, reverse.distance, reverse.pulse_direction);
+
+ action left{-0.5,0.5,3000,-1};
+ move(left.motorA, left.motorB, left.distance, left.pulse_direction);
+
+ action right{0.5,-0.5,3000,1};
+ move(right.motorA, right.motorB, right.distance, right.pulse_direction);
+
+}
+
+
+
+void move(float motorA, float motorB, int distance, int pulse_direction){
+//2 variables to allow for any pulse reading
+float initial_pulse_reading = wheel_B.getPulses();
+float current_pulse_reading = wheel_B.getPulses();
+// - is forward on our robot
+A.speed(motorA);
+B.speed(motorB);
+// loop for moving forward 30cm
+
+if (pulse_direction > 0){
+ while(current_pulse_reading <= (initial_pulse_reading + distance*pulse_direction)){
+ current_pulse_reading = wheel_B.getPulses();
+ }
+}
+else{
+ while(current_pulse_reading >= (initial_pulse_reading + distance*pulse_direction)){
+ current_pulse_reading = wheel_B.getPulses();
+ }
+}
+A.speed(0);
+B.speed(0);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 08 10:35:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Fri Nov 08 10:35:42 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f