File content as of revision 26:6bdfc1856531:

//------------------------------------------------------------------------------------------------------------------------------------
// GROUP 12: THE CHENNE ROBOT (bottle xylophone)
//------------------------------------------------------------------------------------------------------------------------------------
//INCLUDE LIBRARY
//------------------------------------------------------------------------------------------------------------------------------------
#include "mbed.h"
#include "HIDScope.h"
#include "encoder.h"
#include "MODSERIAL.h"

//------------------------------------------------------------------------------------------------------------------------------------
// 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_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

//------------------------------------------------------------------------------------------------------------------------------------
// Defining functions
//------------------------------------------------------------------------------------------------------------------------------------

// FUNCTION 1:  P-controller
double P(double error, const double Kp) //returns error * controller gain
    {
         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;
        scope.set(0,motor2.getPosition());
        scope.send();
        pc.printf("ik doe het positie = %d en error =%d en reference=%d \r\n", position, error, reference);
        return  P(error,motor2_Kp);
    }

//------------------------------------------------------------------------------------------------------------------------------------
// Execution of the programm (MAIN)
//------------------------------------------------------------------------------------------------------------------------------------

// MAIN
int main()                                                                                                          // Begin main loop 1
{
        // CALLIBRATION STARTING 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

        // 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;
                                } 
    
    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
        }                                                                                                          // end of while loop 1
        }                                                                                                          // end of main

//------------------------------------------------------------------------------------------------------------------------------------