Diff: main.cpp

Revision:

2:ee58b6e8eb67

Parent:

1:e0c4625bbbab

--- a/main.cpp	Sun Sep 20 16:31:03 2015 +0000
+++ b/main.cpp	Wed Sep 23 09:21:36 2015 +0000
@@ -1,26 +1,73 @@
 #include "mbed.h"
 #include "encoder.h"
+#include "MODSERIAL.h"
 
- 
-Encoder encoder1(D13,D12);
-DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
-PwmOut motor2speed(D5);
- 
- 
-int main()
+volatile bool looptimerflag;
+
+void setlooptimerflag(void)
 {
-    int position = encoder1.getPosition();
-    Serial pc(USBTX,USBRX);
+    looptimerflag = true;
+} 
+ 
+int main(){
+    //VARIABLES
+    AnalogIn potmeter(A1);
+    AnalogIn potmeter2(A0);
+    DigitalIn button(D8);
+    MODSERIAL pc(USBTX,USBRX);
+    
+    Encoder motor1(D13,D12,true);   // channel A and B from encoder, true - getSpeed works
+    PwmOut pwm_motor1(D6);          // D4 and D5 = motor 2, D7 and D6 = motor 2  
+    DigitalOut dir_motor1(D7);      // 
+    
+    Encoder motor2(D10,D9,true);   // channel A and B from encoder, true - getSpeed works
+    PwmOut pwm_motor2(D5);          // D4 and D5 = motor 2, D7 and D6 = motor 2  
+    DigitalOut dir_motor2(D4);      // 
+    
+    // MOTOR1
+    float goal;
+    float pwm_to_motor;  
+    // MOTOR2
+    float goal2;
+    float pwm_to_motor2;  
+        
+    //CODE
     pc.baud(9600);
-    motor2speed=0.0f;
-    motor2direction=1;
-   
     
-    if(stop_knop.read() == 0) {
-        motor2speed=0.0f;
-    }
-    while (true) {
+    //pwm_motor1.write(0.2f);         // Speed
+    //dir_motor1.write(1);            // Direction
+   
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,0.01);  
+
+    while (1) {
+        while(looptimerflag != true);
+        looptimerflag = false;
         
-          pc.printf("pos: %d, speed %f \r\n",encoder1.getPosition(), encoder1.getSpeed());
+        //MOTOR1
+        goal = (potmeter.read()-0.5)*4200;
+        pc.printf("setpoint: %f, %d, %f \n\r", goal, motor1.getPosition(),motor1.getSpeed());
+        pwm_to_motor = (goal - motor1.getPosition())*.001;   
+    
+        if(pwm_to_motor > 0)
+            dir_motor1 = 0;
+        else
+            dir_motor1 = 1;
+        
+        pwm_motor1.write(abs(pwm_to_motor));
+        
+        //MOTOR2
+        goal2 = (potmeter2.read()-0.5)*4200;
+        //pc.printf("setpoint: %f, %d, %f \n\r", goal2, motor2.getPosition(),motor2.getSpeed());
+        pwm_to_motor2 = (goal2 - motor2.getPosition())*.001;   
+    
+        if(pwm_to_motor2 > 0)
+            dir_motor2 = 0;
+        else
+            dir_motor2 = 1;
+        
+        pwm_motor2.write(abs(pwm_to_motor2));
+           
+
     }
 }
\ No newline at end of file