Diff: main.cpp

Revision:

10:b2742f42de44

Parent:

9:f907915f269c

Child:

11:d31b03b05f59

--- a/main.cpp	Fri Oct 02 10:56:17 2015 +0000
+++ b/main.cpp	Fri Oct 02 11:31:50 2015 +0000
@@ -1,8 +1,10 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "encoder.h"
+#include "HIDScope.h"
 
 Serial pc(USBTX,USBRX);
+HIDScope scope(2);
 
 Ticker          mod;
 
@@ -36,7 +38,7 @@
 
 void move_mot1(double left)
 {
-    if(left < 0.400)
+    if(left < 0.4)
     {
         double calc1 = left - 1;
         double calc2 = abs(calc1);
@@ -44,11 +46,11 @@
         motor1_aan.write(leftin1);
         motor1_rich.write(0);
     }
-    else if(left > 0.4000 && left < 0.6000)
+    else if(left > 0.4 && left < 0.6)
     {
         motor1_aan.write(0);
     }
-    else if(left > 0.6000)
+    else if(left > 0.6)
     {
         double leftin2 = (left-0.6)*2.5;
         motor1_aan.write(leftin2);
@@ -58,7 +60,7 @@
 
 void move_mot2(double right)
 {
-    if(right < 0.4000)
+    if(right < 0.4)
     { 
         double calc3 = right - 1;
         double calc4 = abs(calc3);
@@ -66,11 +68,11 @@
         motor2_aan.write(rightin1);
         motor2_rich.write(0);
     }
-    else if(right > 0.4000 && right < 0.6000)
+    else if(right > 0.4 && right < 0.6)
     {
         motor2_aan.write(0);
     }
-    else if(right > 0.6000)
+    else if(right > 0.6)
     {
         double rightin2 = (right-0.6)*2.5;
         motor2_aan.write(rightin2);
@@ -83,7 +85,16 @@
     int counts1 = motor1_enc.getPosition();
     int counts2 = motor2_enc.getPosition();
     pc.printf("motor 1 counts, %d motor 2 counts %d \n", counts1, counts2);
+
     }
+    
+double get_radians(double counts)
+{
+   double pi = 3.14159265359;
+   double radians = (counts/4200)*2*pi;
+   return radians;   
+}
+
 
 
 int main()
@@ -96,11 +107,19 @@
         double right = potright.read();        
         move_mot1(left);
         move_mot2(right);
+        int counts1 = motor1_enc.getPosition();
+        int counts2 = motor2_enc.getPosition();
+        double radians_1 = get_radians(counts1);
+        double radians_2 = get_radians(counts2); 
+        
         if( button.read() == button_on)
         {
             motor1_enc.setPosition(0);
             motor2_enc.setPosition(0);
         }
+        scope.set(0,radians_1);     
+        scope.set(1,radians_2);     
+        scope.send();
         wait(0.01f);            // is noodzakelijk voor functioneren gradual increase, waarom????
     
     }