Diff: main.cpp

Revision:

9:466dff9ae128

Parent:

8:9b517db94f49

Child:

10:61a7cb3b3aa3

--- a/main.cpp	Mon Oct 01 12:47:32 2018 +0000
+++ b/main.cpp	Fri Oct 05 12:51:06 2018 +0000
@@ -1,23 +1,46 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
-
+#include "math.h"
+#define _USE_MATH_DEFINES
+# define M_PI           3.14159265358979323846  /* pi */
 
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 DigitalOut ledb(LED_BLUE);
 
-PwmOut  pwmpin(PTA2);
-DigitalOut direct(PTB23);
+PwmOut  motor1_pwm(D5);
+PwmOut  motor2_pwm(D6);
+DigitalOut motor1_dir(D4);
+DigitalOut motor2_dir(D7);
 AnalogIn pot1(A0);
+AnalogIn pot2(A1);
 
-QEI encoder(D12,D13,NC,32); 
+QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
+QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);  
 
 InterruptIn  sw2(SW2);
 InterruptIn  sw3(SW3);
 
 MODSERIAL pc(USBTX, USBRX);
 
+Ticker      sample_timer;
+
+const float sample_time = 0.05; //s
+const float sample_frequency = 200; //hz
+
+double countstoangle(int counts){
+    double angle;
+    int counts_abs = abs(counts);
+    if(counts_abs >= 8400){
+       int partial_counts = counts_abs % 8400;
+       angle = (2 * M_PI * partial_counts)/8400;
+       return angle;
+    }else{
+        angle = (2 * M_PI * counts_abs)/8400;
+        return angle;
+    }
+}
 
 int main()
 {
@@ -27,17 +50,30 @@
     
     pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
     
-    pwmpin.period_us(60);
-    pwmpin = 0;
+    motor1_pwm.period_us(60);
+    motor1_pwm = 0;  
+    motor2_pwm.period_us(60);
+    motor2_pwm = 0;  
   
     while (true)
     {
             float pot1_float = pot1.read();
             float pot1_motor = (pot1_float * 2.0f) - 1.0f;
             int mot1_direction = pot1_motor >= 0;
-            pwmpin.write(fabs(pot1_motor));
-            direct = mot1_direction;
-            pc.printf("potmeter: %f, encoder: %i \r\n", pot1_motor, encoder.getPulses());
+            motor1_pwm.write(fabs(pot1_motor));
+            motor1_dir = !mot1_direction;
+            
+            float pot2_float = pot2.read();
+            float pot2_motor = (pot2_float * 2.0f) - 1.0f;
+            int mot2_direction = pot2_motor >= 0;
+            motor2_pwm.write(fabs(pot2_motor));
+            motor2_dir = mot2_direction;
+            
+            double motor1_ang = countstoangle(motor1_enc.getPulses());
+            double motor2_ang = countstoangle(motor2_enc.getPulses());
+            
+            pc.printf("Angle motor 1: %f pi rad, angle motor 2: %f pi rad.\r\n", motor1_ang/(M_PI), motor2_ang/(M_PI));
+            
             wait(0.2f);
     }
 }