Diff: main.cpp

Revision:

4:61d5e7417c4c

Parent:

3:2f75bb307da3

--- a/main.cpp	Tue Oct 27 13:22:39 2015 +0000
+++ b/main.cpp	Tue Oct 27 14:13:48 2015 +0000
@@ -6,13 +6,7 @@
 #include <iostream>
 #include "QEI.h"
 
-// Define pin for motor control
-DigitalOut directionPin(D4);
-PwmOut PWM(D5);
-DigitalIn buttonccw(PTA4);
-DigitalIn buttoncw(PTC6);
 
-// define ticker
 
 
 Serial pc(USBTX, USBRX);
@@ -40,7 +34,7 @@
 }
 
 AnalogIn analog_emg_left(A0);
-//AnalogIn analog_emg_right(A1);
+AnalogIn analog_emg_right(A1);
 double input = 0;
 double filter_signal_hid = 0;
 //double input_right = 0;
@@ -226,8 +220,94 @@
     return(filter_signal);
 }
 
-/*************************************************************motor control******************************************************************************************************/
+/*************************************************************BEGIN motor control******************************************************************************************************/
+// Define pin for motor control
+DigitalOut directionPin(D4);
+PwmOut PWM(D5);
+DigitalIn buttonccw(PTA4);
+DigitalIn buttoncw(PTC6);
+
+QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
+// define ticker
+
+// define rotation direction and begin possition
+const int cw = 1;
+const int ccw = 0;
+double setpoint = 0; //setpoint is in pulses
+
+// saying buttons are not pressed
+const int Buttoncw_pressed = 0;
+const int Buttonccw_pressed = 0;
+
+// Controller gain proportional and intergrator
+const double motor1_Kp = 5; // more or les random number.
+const double motor1_Ki = 0;
+const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
+double motor1_error_integraal = 0; // initial value of integral error
+// Defining pulses per revolution (calculating pulses to rotations in degree.)
+const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4.  10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
+/*
+double Rotation = -2; // rotation 
+double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
+*/
+
+// defining flags
+volatile bool flag_motor = false;
+
+// making function flags.
+void Go_flag_motor()
+{
+    flag_motor = true;
+}
+
 
+// To make a new setpoint 
+double making_setpoint(double direction){
+    if ( cw == direction){
+        setpoint = setpoint + (pulses_per_revolution/40000);
+        }
+        if ( ccw == direction){
+        setpoint = setpoint - (pulses_per_revolution/40000);
+        }
+    return(setpoint);
+    
+    }
+
+// Reusable P controller
+double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
+{ 
+
+  e_int = e_int + Ts * error;
+
+    double PI_output = (Kp * error) + (Ki * e_int);
+    return PI_output;
+}
+// Next task, measure the error and apply the output to the plant
+void motor1_Controller()
+{
+    double reference = setpoint; // setpoint is in pulses
+    double position = wheel.getPulses();
+    double error_pulses = (reference - position); // calculate the error in pulses
+    double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
+    
+    double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
+
+    if(error_pulses > 0) {
+        directionPin.write(cw);
+    
+    }
+    else if(error_pulses < 0) {
+        directionPin.write(ccw);  
+    }
+    else{
+        output = 0;
+        }
+PWM.write(output); // out of the if loop due to abs output
+
+
+}
+
+/*************************************************************END motor control******************************************************************************************************/
 
 void HIDScope_kijken()
 {
@@ -237,21 +317,43 @@
 }
 int main()
 {
-    HIDScope_timer.attach(&Go_flag_HIDScope, 0.002);
-    Filteren_timer.attach(&Go_flag_filteren,0.004);
+    aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
+    HIDScope_timer.attach(&Go_flag_HIDScope, 0.02);
+    Filteren_timer.attach(&Go_flag_filteren,0.04);
     while(1) {
         if(Flag_filteren) {
             Flag_filteren = false;
             filter_signal_hid = Filteren();
+    
         }
 
         if(Flag_HIDScope) {
             Flag_HIDScope = false;
             HIDScope_kijken();
+        
         }
-        if(left_movement) {
-            pc.printf("left = true /n");
+        
+        if(flag_motor) {
+            flag_motor = false;
+            motor1_Controller();
+        
+        }
+
+    
+
+// Pussing buttons to get input signal
+
+    if(left_movement){
+      setpoint = making_setpoint(cw);
+      
+        }
+if(buttonccw.read() == Buttonccw_pressed) {
+          setpoint =  making_setpoint(ccw);
         }
     }
 }
 
+
+    
+   
+  
\ No newline at end of file