Diff: main.cpp

Revision:

0:3e435f58182d

diff -r 000000000000 -r 3e435f58182d main.cpp
--- /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);
+}
+