Diff: main.cpp

Revision:

0:f6306e179750

Child:

1:2f23368e8638

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 07 17:20:12 2015 +0000
@@ -0,0 +1,170 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
+// #include "HIDScope.h"
+
+Serial pc(USBTX,USBRX);
+// HIDScope scope(4);          // definieerd het aantal kanalen van de scope
+
+// Define Tickers and stuff
+Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
+double controlfreq = 10 ;    // controlloops frequentie (Hz)
+double controlstep = 1/controlfreq; // timestep derived from controlfreq
+
+//MOTOR INPUTPINS
+// motor 1
+PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
+DigitalOut motor1_rich(D7); // digitaal signaal voor richting
+// motor 2
+PwmOut motor2_aan(D5);
+DigitalOut motor2_rich(D4);
+
+// ENCODER
+Encoder motor1_enc(D12,D11);        // encoder outputpins
+Encoder motor2_enc(D10,D9);
+
+//POTMETERS
+AnalogIn potright(A0);      // define the potmeter outputpins
+AnalogIn potleft(A1);
+
+// RESETBUTTON              
+DigitalIn   button(PTA4);      // defines the button used for a encoder reset
+int button_pressed = 0;
+
+int reference1 = 0;         // set the reference position of the encoders
+int reference2 = 0;
+
+// CONTROLLER SETTINGS
+const double K1 = 1;       // P constant motorcontrolle 1
+const double K2 = 1;        // p constant motorcontroller 2
+
+
+// SETPOINT SETTINGS
+// define storage variables for setpoint values
+double c_setpoint1 = 0;
+double c_setpoint2 = 0;
+// define the maximum rate of change for the setpoint (velocity)
+double Vmax = 5;            // rad/sec
+
+
+
+////////////////////////////////////////////////////////////////
+/////////////////// START OF SIDE FUNCTIONS ////////////////////
+//////////////////////////////////////////////////////////////
+// these functions are tailored to perform 1 specific function
+
+
+// counts 2 radians
+// this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
+// It has been set up for standard 2X DECODING!!!!!!
+double get_radians(double counts)       
+{
+    double pi = 3.14159265359;
+    double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
+    return radians;
+}
+
+
+// This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_setpoint)
+// to create a reference that moves with a variable speed. It is now set up for potmeter values.  
+double setpoint_f(double input, double &c_setpoint)
+{
+    double offset = 0.5, gain = 2;                
+    double potset = (input-offset)*gain;
+    double setpoint = c_setpoint + potset * controlstep * Vmax ;
+        c_setpoint = setpoint;
+    return setpoint;
+}
+
+
+// This function takes the inputvalue and ensures it is between -1 and 1
+// this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
+// I am aware it has unnecesary steps but it works.
+double outputlimiter (double output)
+{
+    if(output>1)                                
+    {
+        output = 1;
+    }
+    else if(output < 1 && output > 0)
+    {
+        output = output;
+    }
+    else if(output > -1 && output < 0)
+    {
+        output = output;
+    }
+    else if(output < -1)
+    {
+        (output = -1);
+    }
+    return output;
+}
+
+
+// this function is a simple K control called by the motor function
+double K_control(double error,double K)
+{
+    double output = K*error;                            // controller output K*e
+    output = outputlimiter(output);             // limit the output to -1->1
+    return output;
+}
+
+/////////////////////////////////////////////////////////////////////
+////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
+///////////////////////////////////////////////////////////////////
+// these functions are used to control all aspects of a single electric motor and are called by the main function
+
+// MOTOR 1
+void motor1_control()
+{
+    double setpoint1 = setpoint_f(potright.read(), c_setpoint1);      // determine the setpoint that has been set by the inputsignal
+    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
+    double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
+    // scope.set(0,setpoint1);                                                                                                              // disabled because HIDScope is acting strangely
+    // scope.set(1,rads1);
+    // scope.send();
+    double output1 = K_control(error1, K1);        // bereken de controller output voor motor 1(zie hierboven)
+    if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
+        motor1_rich.write(0);
+        motor1_aan.write(output1);
+    } else if(output1 < 0) {
+        motor1_rich.write(1);
+        motor1_aan.write(abs(output1));
+    }
+}
+
+// MOTOR 2
+void motor2_control()
+{
+    double setpoint2 = setpoint_f(potleft.read(), c_setpoint2);      // determine the setpoint that has been set by the inputsignal
+    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
+    double error2 = (setpoint2 - rads2);                       // determine the error (reference - position)
+    // scope.set(3,setpoint2);                                                                                                              // disabled because HIDScope is acting strangely
+    // scope.set(4,rads2);
+    // scope.send();
+    double output2 = K_control(error2, K2);        // bereken de controller output voor motor 1(zie hierboven)
+    if(output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
+        motor2_rich.write(0);
+        motor2_aan.write(output2);
+    } else if(output2 < 0) {
+        motor2_rich.write(1);
+        motor2_aan.write(abs(output2));
+    }
+}
+
+
+int main()
+{
+    // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop.
+    controller1.attach(&motor1_control, controlstep);
+    controller2.attach(&motor2_control, controlstep);
+    while(true) 
+    {
+        if(button.read() == button_pressed)             // reset the encoder to reference position
+        {
+            motor1_enc.setPosition(reference1);
+            motor2_enc.setPosition(reference2);
+        }
+    }
+}
\ No newline at end of file