Diff: main.cpp

Revision:

3:dcc4cebba0d7

Parent:

2:11076f69e0a7

Child:

4:db3dad7e53e3

--- a/main.cpp	Tue Oct 21 13:48:51 2014 +0000
+++ b/main.cpp	Wed Oct 22 13:04:35 2014 +0000
@@ -13,75 +13,57 @@
 #define M2_PWM PTA5 //blauw
 #define M2_DIR PTA4 //groen
 
-//#define POT_AVG 50
+Serial pc(USBTX, USBRX);
+DigitalOut led1(LED_RED); 
+
+
 void clamp(float * in, float min, float max);
 float pid(float setpoint, float measurement);
 volatile bool looptimerflag;
-//float potsamples[POT_AVG];
 HIDScope scope(6);
 
+
 void setlooptimerflag(void)
 {
     looptimerflag = true;
 }
 
+
+
 int main()
-{   //Let op dat de jumpers goed staan als het motortje niet wilt draaien. De E1 jumper moet onder de nummer 7 pin. De locatie van de M1 pin
-    //bepaalt of de motor CW of CCW draait. 
+{
+    //Let op dat de jumpers goed staan als het motortje niet wilt draaien. De E1 jumper moet onder de nummer 7 pin. De locatie van de M1 pin
+    //bepaalt of de motor CW of CCW draait.
     //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
-    AnalogIn potmeter(PTC2);
+
+    //motor 25D
     Encoder motor1(PTD3,PTD5); //wit, geel
-    PwmOut pwm_motor1(M1_PWM); // PTC8, blauw,  /*PwmOut to motor driver*/
+    PwmOut pwm_motor1(M2_PWM); // PTC8, blauw,  /*PwmOut to motor driver*/
     pwm_motor1.period_us(75); //10kHz PWM frequency
-    DigitalOut motordir1(M1_DIR); //PTC9, groen
-    
-  
+    DigitalOut motordir1(M2_DIR); //PTC9, groen
+
+//motor2 37D
     Encoder motor2(PTD2, PTD0); //wit, geel
-    PwmOut pwm_motor2(M2_PWM); //PTA5, blauw
+    PwmOut pwm_motor2(M1_PWM); //PTA5, blauw
     pwm_motor2.period_us(75);
-    DigitalOut motordir2(M2_DIR); //PTA4, groen
-    
-    Ticker looptimer;
-    looptimer.attach(setlooptimerflag,TSAMP);
-    
-    
+    DigitalOut motordir2(M1_DIR); //PTA4, groen
+
+
     while(1) {
-        int16_t setpoint;
-        float new_pwm;
-        /*wait until timer has elapsed*/
-        while(!looptimerflag);
-        looptimerflag = false; //clear flag
-        /*potmeter value: 0-1*/
-        setpoint = (potmeter.read()-.5)*500;        
-        /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
-        new_pwm = pid(setpoint, motor1.getPosition());
-        clamp(&new_pwm, -1,1);
-        scope.set(0, setpoint);
-        scope.set(4, new_pwm);
-        scope.set(5, motor1.getPosition());
-        // ch 1, 2 and 3 set in pid controller */
-        scope.send();
-        if(new_pwm < 0)
-            motordir1 = 0;
-        else
-            motordir1 = 1;
-        pwm_motor1.write(abs(new_pwm));
         
-         if(new_pwm < 0)
-            motordir2 = 0;
-        else
-            motordir2 = 1;
-        pwm_motor2.write(abs(new_pwm));
+        pwm_motor1.write(1); 
+        wait(3);
+        pwm_motor1.write(0); 
+        wait(3); 
     }
 }
 
-
 //clamps value 'in' to min or max when exceeding those values
 //if you'd like to understand the statement below take a google for
 //'ternary operators'.
 void clamp(float * in, float min, float max)
 {
-    *in > min ? *in < max? : *in = max: *in = min;
+*in > min ? *in < max? : *in = max: *in = min;
 }