Diff: main.cpp

Revision:

1:faa90344cdd8

Parent:

0:57aa29917d4c

Child:

2:ca645c585a03

--- a/main.cpp	Tue Nov 01 11:50:51 2016 +0000
+++ b/main.cpp	Wed Nov 02 10:50:35 2016 +0000
@@ -2,6 +2,8 @@
 #include "QEI.h"  //bieb voor encoderfuncties in c++
 #include "MODSERIAL.h" //bieb voor modserial
 #include "BiQuad.h"
+#include "HIDScope.h"
+#include "math.h"
 
 InterruptIn sw3(SW3);
 DigitalIn sw2(SW2);
@@ -17,6 +19,7 @@
 DigitalOut richting_motor2(D7);
 PwmOut pwm_motor2(D6);
 BiQuad PID_controller;
+HIDScope Scope(1);
 
 //constanten voor de encoder
 const int CW = 2.5; //definitie rechtsom 'lage waarde'
@@ -42,7 +45,7 @@
 volatile float d_ref_new;
 const float w_ref = 3;
 const double Fs = 0.001;
-const double Kp = 5;
+const double Kp = 0.5;
 const double Ki = 5;
 const double Kd = 5; 
 const double N = 100;
@@ -52,6 +55,7 @@
 volatile float voltage_motor1=0.18;
 volatile double error1;
 volatile double ctrlOutput;
+volatile double sintel =0;
 
 void reference(){
 d_ref = d_ref + w_ref * Fs;
@@ -60,6 +64,10 @@
 void m1_controller(){
     error1 = d_ref-rev_counts_motor1_rad;
     ctrlOutput = PID_controller.step(error1);
+    }
+void sin_teller(){
+    sintel=sintel+0.01;
+    }
 // if (ctrlOutput > 1){
 //        ctrlOutput =1;
  //       }
@@ -69,7 +77,7 @@
  //   else{
  //       ctrlOutput = ctrlOutput;
  //       }
-    }
+    
 
 
 
@@ -83,19 +91,23 @@
     
     while (true) {
         if (button_cw == 0){
-
+ t.attach(&sin_teller,1);
 richting_motor1 = 1;
-pwm_motor1 = voltage_motor1;
+pwm_motor1 = sin(sintel*10*3.141592653);
+pc.printf("pwm_motor is %f\r\n", pwm_motor1);
+pc.printf("sintel is %f\r\n", sintel);
 counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); 
 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718; 
-tick.attach(&reference,Fs);  
-PID_controller.PIDF(Kp,Ki,Kd,N,Fs);
-controllerticker.attach(&m1_controller,Fs);
-voltage_motor1=ctrlOutput;
-pc.printf("%f", voltage_motor1);
-pc.printf("   %f", d_ref);
-pc.printf("   %f \r\n", rev_counts_motor1_rad);
+//tick.attach(&reference,Fs);  
+//PID_controller.PIDF(Kp,Ki,Kd,N,Fs);
+//controllerticker.attach(&m1_controller,Fs);
+//voltage_motor1=ctrlOutput;
+//pc.printf("%f", voltage_motor1);
+//pc.printf("   %f", d_ref);
+//pc.printf("   %f \r\n", rev_counts_motor1_rad);
+Scope.set(0, rev_counts_motor1_rad);
+Scope.send();
 }
 
 else{