Revision 25:039e2b6429ad, committed 2018-11-02

Comitter:

1856413

Date:

Fri Nov 02 11:26:18 2018 +0000

Parent:

24:f9dccbc1fc7e

Commit message:

Use this script for DEMOMODE

Changed in this revision

--- a/main.cpp	Wed Oct 31 21:11:28 2018 +0000
+++ b/main.cpp	Fri Nov 02 11:26:18 2018 +0000
@@ -6,12 +6,13 @@
 MODSERIAL pc(USBTX, USBRX);
 DigitalOut motor1DirectionPin(D7);
 DigitalOut motor2DirectionPin(D4);
-DigitalOut Led(LED_GREEN);
+DigitalOut Led1(LED_GREEN);
+DigitalOut Led2(LED_GREEN);
 FastPWM motor1MagnitudePin(D6);
 FastPWM motor2MagnitudePin(D5);
 AnalogIn potMeter1(A4);
 AnalogIn potMeter2(A5);
-InterruptIn button2(D3);
+InterruptIn button(SW3);
 QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
 QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
 
@@ -29,54 +30,57 @@
 volatile double motorValue2 = 0.01;
 volatile double errorM1 = 0.0;
 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
+volatile double Kp2 = 0.34;
 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
 volatile double Kd = 0.0;
 volatile double Ts = 0.01;
 
 //------------------------------------------------------------------------------
+void Stopmotor() {
+    // Motor PWM
+    motor1MagnitudePin = 0.0f; // Range van 0.0f tot 1.0f
+    motor2MagnitudePin = 0.0f;
+    
+    Led1 = 1;
+    Led2 = 0;
+}
 
-double GetReferencePosition1()
-{
+
+double GetReferencePosition1() {
     double potMeter1In = potMeter1.read();
     referencePosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
     return referencePosition1;
 }
 
-double GetReferencePosition2()
-{
+double GetReferencePosition2() {
     double potMeter2In = potMeter2.read();
     referencePosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
     return referencePosition2;
 }
 
-double GetMeasuredPosition1()
-{
+double GetMeasuredPosition1() {
     int counts1 = Encoder1.getPulses();
     double counts1d = counts1*1.0f;
     measuredPosition1 = ( counts1d / (8400)) * 6.28; // Rotational position in radians
     return measuredPosition1;
 }
 
-double GetMeasuredPosition2()
-{
+double GetMeasuredPosition2() {
     int counts2 = Encoder2.getPulses();
     double counts2d = counts2*1.0f;
     measuredPosition2 = ( counts2d / (8400)) * 6.28;
     return measuredPosition2;
 }
 
-double FeedbackControl1(double Error1)
-{
+double FeedbackControl1(double Error1) {
     static double Error_integral1 = 0;
     static double Error_prev1 = Error1;
-    //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
     // Proportional part:
-    //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
     double u_k1 = Kp * Error1;
     // Integral part:
     Error_integral1 = Error_integral1 + Error1 * Ts;
     double u_i1 = Ki * Error_integral1;
-    // Derivative part
+    // Derivative part:
     double Error_derivative1 = (Error1 - Error_prev1)/Ts;
     double u_d1 = Kd * Error_derivative1;
     Error_prev1 = Error1;
@@ -84,14 +88,13 @@
     return u_k1 + u_i1 + u_d1; //motorValue
 }
 
-double FeedbackControl2(double Error2)
-{
+double FeedbackControl2(double Error2) {
     static double Error_integral2 = 0;
     static double Error_prev2 = Error2;
     //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
     // Proportional part:
     //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
-    double u_k2 = Kp * Error2;
+    double u_k2 = Kp2 * Error2;
     // Integral part:
     Error_integral2 = Error_integral2 + Error2 * Ts;
     double u_i2 = Ki * Error_integral2;
@@ -103,8 +106,7 @@
     return u_k2 + u_i2 + u_d2; //motorValue
 }
 
-void SetMotor1(double motorValue1)
-{
+void SetMotor1(double motorValue1) {
     // Given -1<=motorValue<=1, this sets the PWM and direction
     // bits for motor 1. Positive value makes motor rotating
     // clockwise. motorValues outside range are truncated to
@@ -123,8 +125,7 @@
     }
 }
 
-void SetMotor2(double motorValue2)
-{
+void SetMotor2(double motorValue2) {
     // Given -1<=motorValue<=1, this sets the PWM and direction
     // bits for motor 1. Positive value makes motor rotating
     // clockwise. motorValues outside range are truncated to
@@ -144,8 +145,7 @@
 }
 //-----------------------------------------------------------------------------
 // Tickers
-void MeasureAndControl1(void)
-{
+void MeasureAndControl1(void) {
     // This function determines the desired velocity, measures the
     // actual velocity, and controls the motor with
     // a simple Feedback controller. Call this from a Ticker.
@@ -156,8 +156,7 @@
     SetMotor1(motorValue1);
 }
 
-void MeasureAndControl2(void)
-{
+void MeasureAndControl2(void) {
     // This function determines the desired velocity, measures the
     // actual velocity, and controls the motor with
     // a simple Feedback controller. Call this from a Ticker.
@@ -166,8 +165,8 @@
     motorValue2 = FeedbackControl2(referencePosition2 - measuredPosition2);
     SetMotor2(motorValue2);
 }
-void printen()
-{
+
+/*void printen() {
     pc.baud (115200);
     pc.printf("Referenceposition POT1 =  %f \t Referenceposition POT2 = %f \r\n", referencePosition1, referencePosition2);
     pc.printf("Measured position 1 = %f \t Measured position 2 = %f \r\n", measuredPosition1, measuredPosition2);
@@ -176,10 +175,10 @@
     //pc.printf("Proportional gain %f \r\n", Kp);
     //pc.printf("Integral gain %f \r\n", Ki);
     //pc.printf("Derivative gain %f \r\n", Kd);
-}
+}*/
+
 //-----------------------------------------------------------------------------
-int main()
-{
+int main() {
     //Initialize once
     pc.baud(115200);
     motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz.
@@ -187,12 +186,14 @@
     
     MeasureControl1.attach(MeasureAndControl1, 0.01);
     MeasureControl2.attach(MeasureAndControl2, 0.01);
-    print.attach(printen, 1.0);
+    /*print.attach(printen, 1.0);*/
 
     //Other initializations
+    button.fall(Stopmotor);
 
     while(true) {
-        Led = !Led;
+        Led2 = 1;
+        Led1 = !Led1;
         wait(2);
     }
 }