Diff: main.cpp

Revision:

25:75cce8f86272

Parent:

24:465efbd884d5

Child:

26:6bdfc1856531

--- a/main.cpp	Tue Sep 22 15:06:17 2015 +0000
+++ b/main.cpp	Tue Sep 22 21:36:28 2015 +0000
@@ -1,32 +1,51 @@
+//------------------------------------------------------------------------------------------------------------------------------------
+// GROUP 12: THE CHENNE ROBOT (bottle xylophone)
+//------------------------------------------------------------------------------------------------------------------------------------
+//INCLUDE LIBRARY
+//------------------------------------------------------------------------------------------------------------------------------------
 #include "mbed.h"
 #include "HIDScope.h"
 #include "encoder.h"
 #include "MODSERIAL.h"
 
-//Motor 2
-DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
-PwmOut motor2speed(D5); // D5 is an input on the motor that controls the speed of motor number 2
-DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
-Encoder motor2(D13,D12,true); // The encoder is able to read and set position/speed values
-MODSERIAL pc(USBTX,USBRX); // This input is used to send data to the pc
-Ticker TickerController; // This adds a Ticker to the function +TickerController
-HIDScope scope(2);
-//AnalogIn potmeter2(A0); /NIEUW
+//------------------------------------------------------------------------------------------------------------------------------------
+// Defining inputs/outputs/constants
+//------------------------------------------------------------------------------------------------------------------------------------
+
+// MOTOR 2 SIGNALS
+DigitalOut motor2direction(D4);        //D4 and D5 are Motor 2 inputs
+PwmOut motor2speed(D5);                // D5 is an input on the motor that controls the speed of motor number 2
+DigitalIn button(PTA4);                // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
+Encoder motor2(D13,D12,true);          // The encoder is able to read and set position/speed values
+Ticker TickerController;               // This adds a Ticker to the function +TickerController
+//AnalogIn potmeter2(A0);              // NOT IMPLEMENTED YET
+
+
+// OUTPUT DATA
+MODSERIAL pc(USBTX,USBRX);             // This input is used to send data to the pc
+HIDScope scope(2);                     // This adds two HIDScope channels (1) and (0) to display signals to HIDScope
+
 
-// Defining constants
-double counts_per_revolution=4200; 
-double degrees_per_turn=360;
-double counts_per_degree=counts_per_revolution/degrees_per_turn; //11.67 counts/degree
-const double motor2_Kp = 1; //controller gain which will be multiplied with the error (*how fast will the error be corrected)
-const double reference = 400;
+// DEFINING CONSTANTS
+double counts_per_rev=4200;                                 // Counts per revolution DERIVED FROM TESTS (NOG DOEN)
+double degrees_per_turn=360;                                // Degrees for one revolution
+double counts_per_degree=counts_per_rev/degrees_per_turn;   // 11.67 counts/degree //divide counts by 11.67 to obtain degrees
+const double motor2_Kp = 1;                                 // Controller gain which will be multiplied with the error (*how fast will the error be corrected)
+const double reference = 400;                               // Reference value to what position (counts) shall the motor return
 
-// Function P-controller
+//------------------------------------------------------------------------------------------------------------------------------------
+// Defining functions
+//------------------------------------------------------------------------------------------------------------------------------------
+
+// FUNCTION 1:  P-controller
 double P(double error, const double Kp) //returns error * controller gain
-{
-    return Kp*error; 
-}
-// Function that calls the P-controller
-double motor2_controller() // Void function that compares the current position with a reference value and outputs 
+    {
+         return Kp*error; 
+    }
+
+
+// FUNCTION 2:  Calculates the errors and puts the value into function 1
+double motor2_controller() 
     {
         double position=motor2.getPosition(); //current motor position
         double error=reference-position;
@@ -36,37 +55,64 @@
         return  P(error,motor2_Kp);
     }
 
+//------------------------------------------------------------------------------------------------------------------------------------
+// Execution of the programm (MAIN)
+//------------------------------------------------------------------------------------------------------------------------------------
+
 // MAIN
-int main()
+int main()                                                                                                           // Begin main loop 1
 {
-    pc.baud(9600); // baud rate at which the information is processed to the computer
-    //motor2.setPosition(0); // calls the current position of the motor zero
-    while(true) {
-        if (button.read() < 0.5) {   //if button pressed
-            motor2direction = 0; // zero is clockwise (front view)
-            motor2speed = 0.5f; // motorspeed
-            pc.printf("positie = %d \r\n", motor2.getPosition());
-        } else {                  // If button is not pressed
-            double output_motor = motor2_controller();
-            while(output_motor>0){
-            motor2speed=output_motor;
-            motor2direction=1;
-            break;}
-            while(output_motor<0){
-            motor2speed=fabs(output_motor);
-            motor2direction=0;
-            break;}
-        {
-            pc.printf("else loop controller");
-            }
-        }
+        // CALLIBRATION BEGIN POSITION
+                pc.baud(9600);           // baud rate at which the information is processed to the computer
+                //motor2.setPosition(0); // calls the current position of the motor zero
     
-// Piece of code that keeps the value of the position between the counts per revolution
-        while ((motor2.getPosition()>counts_per_revolution) || (motor2.getPosition()<-counts_per_revolution)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
-        {
-            motor2.setPosition(0);
-            pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
-            break;
-        } 
-    }
-    }
\ No newline at end of file
+    while(true) {                                                                                                    // while loop 1
+        // IF BUTTON IS PRESSED
+            if (button.read() < 0.5)                                                                                 
+                {                                                                                                    // start if else loop 1
+                        motor2direction = 0;  // clockwise motor rotation (front view)
+                        motor2speed = 0.5f;   // motorspeed                
+                                        
+                        pc.printf("positie = %d \r\n", motor2.getPosition());
+                }    
+        // IF BUTTON IS NOT PRESSED
+            else   
+                {                 
+                        double output_motor = motor2_controller(); //extract value from motor2_controller
+                        
+                                //Case 1
+                                    while(output_motor>0)          // Value to correct for by the motor >0 
+                                { 
+                                    motor2speed=output_motor;      //then apply this value
+                                    motor2direction=1;             //to the motor in anti-clockwise direction
+                                    break;                         // abort the loop and continue the script
+                                } 
+                                    
+                                //Case 2
+                                    while(output_motor<0)          // Value to correct for by the motor <0
+                                {
+                                    motor2speed=fabs(output_motor);// then apply the absolute value 
+                                    motor2direction=0;             // to the motor in clockwise direction
+                                    break;                         // then abort the loop and continue the script
+                                }
+        // ATTACHING TICKER
+                                {
+                                    TickerController.attach(&motor2_controller,0.01f);//100Hz
+                                    pc.printf("else loop controller");
+                                }
+                }                                                                                               // end of if-else loop 1
+    
+        // CALLIBRATION POSITION OF MOTOR
+                    // The position is limited to number of counts per revolution
+            
+           while ((motor2.getPosition()>counts_per_rev) || (motor2.getPosition()<-counts_per_rev)) 
+                    // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
+                                {
+                                    motor2.setPosition(0);
+                                    pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
+                                    break;
+                                } 
+        }                                                                                                             // end of while loop 1
+        }                                                                                                             // end of main
+
+//------------------------------------------------------------------------------------------------------------------------------------
\ No newline at end of file