Diff: main.cpp

Revision:

8:676646c50aeb

Parent:

7:bafc32b576c4

Child:

9:6cee14e0e323

--- a/main.cpp	Mon Oct 31 10:42:56 2016 +0000
+++ b/main.cpp	Mon Oct 31 12:38:14 2016 +0000
@@ -26,6 +26,10 @@
 InterruptIn button3(PTC6);                  // SW2
 InterruptIn button4(PTA4);                  // SW3
 
+// Setup botton values
+bool button1_value = false;
+bool button2_value = false;
+
 // Set motor Pinouts
 DigitalOut motor1_dir(D4);
 FastPWM motor1_pwm(D5);
@@ -46,14 +50,20 @@
 
 // Constants for control
 volatile const int   COUNTS_PER_REV = 8400;
-volatile const float DEGREES_PER_PULSE = 8400 / 360;
-volatile const float CONTROLLER_TS = 0.01; // 1000 Hz
+volatile const double DEGREES_PER_PULSE = 8400 / 360;
 
 // Set initial Kp and Ki
 volatile double Kp = 0.01; // last known good Kp: 0.01
-volatile double Ki = 0.0;   // last known good Ki: 0.0025
+volatile double Ki = 0.0025;   // last known good Ki: 0.0025
 volatile double Kd = 0.0; // last known good Kd: 0.0
 
+// Set frequencies for sampling, calc and control.
+volatile const double SAMPLE_F = 500;               // 500 Hz
+volatile const double SAMPLE_TS = 1.0/SAMPLE_F;
+volatile const double CALC_F = 100.0;               // 100 Hz
+volatile const double CALC_TS = 1.0/CALC_F;
+volatile const double CONTROLLER_TS = 0.01;         // 100 Hz
+
 volatile double Ts = 0.01;
 volatile double N = 100;
 
@@ -93,9 +103,10 @@
 volatile double Theta2Gear = 60.0;
 volatile double Theta1 = Theta1Gear*42/12;           // Beginpositie van MotorHoek
 volatile double Theta2 = Theta2Gear*42/12;
-double Fr_Speed_Theta = 100.0;              // frequentie in Hz waarmee theta wordt uigerekend
+
 bool Calibration = true;                    // beginnen met calibreren
 volatile bool Stepper_State = false;        // false = we zijn niet bezig met flippen
+volatile double Speed = 10;                 // mm/s the position is changed
 
 /////////////////////// functions for filters /////////////////////////////////////////////
 void sample()
@@ -190,9 +201,10 @@
 
 void FunctieFlipper();      // declarate function, function discribed below
 // berekenen van de nieuwe x en y waardes
-bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State) 
+void CalcXY () 
 {
-    double Step = 5/Fr_Speed_Theta ;  //10 mm per seconde afleggen
+    // calc steps in mm
+    double Step = Speed/CALC_F ;  //10 mm per seconde afleggen
     
     if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {   
         Stepper_State = true;     // we zijn aan het flipper dus geen andere dingen doen
@@ -230,8 +242,6 @@
       
     //scope.set(2,x);
     //scope.set(3,y);
-    
-    return Stepper_State;
 }    
 
 // diagonaal berekenen voor linker arm
@@ -317,36 +327,214 @@
 }
 
 void CalculationsForTheta () {
-    sample();
     double Prev_x = Calc_Prev_x ();
     double Prev_y = Calc_Prev_y ();
-    double Calc_Prev_Theta1_Gear ();
-    double Calc_Prev_Theta2_Gear ();
-    bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State);
+    double Prev_Theta1_Gear = Calc_Prev_Theta1_Gear ();
+    double Prev_Theta2_Gear = Calc_Prev_Theta2_Gear ();
+    CalcXY();
     double Dia1 = CalcDia1 (x, y);
     double Dia2 = CalcDia2 (x, y);
     CalcTheta1 (Dia1);
-    CalcTheta2 (Dia2); 
-    AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y);
-
-    //scope.send();
+    CalcTheta2 (Dia2);
+    AngleLimits (Prev_Theta1_Gear, Prev_Theta2_Gear, Prev_x, Prev_y);       // laatste check
 }    
 
-// als de button ingedrukt wordt dan stoppen we met calibreren        
-void EndCalibration () {
-        Calibration = false;
-}        
+////////////////////////// Code for control /////////////////////////////////////
+
+double m1_GetPosition()
+{
+    int countsCW = m1_EncoderCW.getPulses();
+    int countsCCW= m1_EncoderCCW.getPulses();
+    int net_counts=countsCW-countsCCW;
+    double Position=(net_counts*360.0)/COUNTS_PER_REV;
+    return Position;
+}
+
+double m2_GetPosition()
+{
+    int countsCW = m2_EncoderCW.getPulses();
+    int countsCCW= m2_EncoderCCW.getPulses();
+    int net_counts=countsCW-countsCCW;
+    double Position=(net_counts*360.0)/COUNTS_PER_REV;
+    return Position;
+}
+
+// Position control when calibrating
+double m1_GetPosition_cal()
+{
+    int countsCW = m1_EncoderCW.getPulses();
+    int countsCCW= m1_EncoderCCW.getPulses();
+    int net_counts=countsCW-countsCCW;
+    double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
+    return Position;
+}
+
+// Position control when calibrating
+double m2_GetPosition_cal()
+{
+    int countsCW = m2_EncoderCW.getPulses();
+    int countsCCW= m2_EncoderCCW.getPulses();
+    int net_counts=countsCW-countsCCW;
+    double Position=(net_counts*360.0)/COUNTS_PER_REV+210.0f; // calibrated position is 210 degrees
+    return Position;
+}
+double m1_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m1_v1, double &m1_v2)
+{
+    double a1 = -4/(N*Ts+2),
+    a2 = -1*(N*Ts - 2)/(N*Ts+2), 
+    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
+    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
+    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+    
+    double v = error - a1*m1_v1 - a2*m1_v2;
+    double u = b0*v + b1*m1_v1 + b2*m1_v2;
+    m1_v2 = m1_v1; m1_v1 = v;
+    return u;
+}
+
+double m2_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m2_v1, double &m2_v2)
+{
+    double a1 = -4/(N*Ts+2),
+    a2 = -1*(N*Ts - 2)/(N*Ts+2), 
+    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
+    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
+    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
+    
+    double v = error - a1*m1_v1 - a2*m1_v2;
+    double u = b0*v + b1*m1_v1 + b2*m1_v2;
+    m1_v2 = m1_v1; m1_v1 = v;
+    return u;
+}
+
+
 
-//////////////// functies voor aansturing motoren /////////////////////////////// 
-void FunctieFlipper () {
-    Stepper_State = false;
+void SetMotor(int motor_number, double MotorValue)
+{
+    // 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
+    // within range
+    if (motor_number == 1)
+    {
+        if (MotorValue >=0)
+        {
+            motor1_dir=0;
+        }
+        else
+        {
+            motor1_dir=1;
+        }
+        if (fabs(MotorValue)>1){
+            motor1_pwm.write(1);
+        }
+        else
+        {
+            motor1_pwm.write(abs(MotorValue));
+        }
+    }
+    else
+    {  
+        if (MotorValue >=0)
+        {
+            motor2_dir=0;
+        }
+        else
+        {
+            motor2_dir=1;
+        }
+        if (fabs(MotorValue)>1){
+            motor2_pwm.write(1);
+        }
+        else
+        {
+            motor2_pwm.write(abs(MotorValue));
+        }
+    }   
+}
+
+void BlinkLed(){
+    led = not led;
+}
+
+void Controller_motor()
+{
+    // get the actual position
+    double m1_Position = m1_GetPosition_cal();
+    double m2_Position = m2_GetPosition_cal();
+    // Set position scopes
+    scope.set(0,x);
+    scope.set(1,y);
+    scope.set(2,Theta1);
+    scope.set(3,Theta2);
+    /*
+    scope.set(0,Theta1);
+    scope.set(1,m1_Position);
+    scope.set(3,Theta2);
+    scope.set(4,m2_Position);
+    */
+    //scope.set(2,m1_Position);
+    //scope.set(4,m2_Position);
+    // calc the error
+    double m1_error = Theta1 - m1_Position;
+    double m2_error = Theta2 - m2_Position;
+    //scope.set(2,m1_error);
+    //scope.set(6,m2_error);
+    // calc motorvalues for controller;
+    double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
+    double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
+    scope.set(4,m1_MotorValue);
+    scope.set(5,m2_MotorValue);
+    // Set the motorvalues
+    SetMotor(1, m1_MotorValue);
+    SetMotor(2, m2_MotorValue);
+    // Set motorvalues for scope
+    // Send data to HIDScope
+    scope.send();
+}
+
+void PotControl()
+{
+    double Motor1_Value = (pot1.read() - 0.5f)/2.0f;
+    double Motor2_Value = (pot2.read() - 0.5f)/2.0f;
+    //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
+    //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
+    double m1_Position = m1_GetPosition();
+    double m2_Position = m2_GetPosition();
+    scope.set(0, Motor1_Value);
+    scope.set(1, m1_Position);
+    scope.set(2, Motor2_Value);
+    scope.set(3, m2_Position);
+    scope.send();
+    // Write the motors
+    SetMotor(1, Motor1_Value);
+    SetMotor(2, Motor2_Value);
+}
+
+void ResetEncoders()
+{
+    m1_EncoderCW.reset();
+    m1_EncoderCCW.reset();
+    m2_EncoderCW.reset();
+    m2_EncoderCCW.reset();
+}
+
+void Button1Switch()
+{
+    button1_value = not button1_value;
+}
+
+void Button2Switch()
+{
+    button2_value = not button2_value;
 }
 
 ////////////////////////// main loop ////////////////////////////////////////////
 int main()
-{   
+{
+    // Setup
+    // Set baud connection with PC
     pc.baud(SERIAL_BAUD);
-    pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
+    pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
     
     /* making the biquad chain for filtering the emg
     notch and high pass */
@@ -354,29 +542,62 @@
     bqc2.add( &bq4 ).add( &bq5 );
     bqc3.add( &bq7 ).add( &bq8 );
     
-    // Tickers
-    Ticker      sample_timer;
-    Ticker      TickerCalculationsForTheta;
+    // Setup Tickers
+    Ticker      SampleTicker;
+    Ticker      CalculationsTicker;
+    Ticker      BlinkLedTicker;
+    Ticker      PIDControlTicker;
+    Ticker      PotControlTicker;
     
+    // Setup Blinking LED to blink every 0.5 sec
+    led = 1;
+    BlinkLedTicker.attach(BlinkLed,0.5);
+    
+    // Setup motor PWM speeds
+    motor1_pwm.period(1.0/1000);
+    motor2_pwm.period(1.0/1000);
     
-    // Calibreren
-    while (Calibration == true) {
-        //potmeter dingen aanpassen
+    // Setup Interruptin.fall
+    button1.fall(Button1Switch);
+    //button2.fall(xxx);
+    //button3.fall(xxx);
+    //button4.fall(xxx);
         
-        pc.printf("calibreren is bezig\r\n");
-        button1.fall(&EndCalibration);
-    }
+    // Actual code starts here
+    // Start positioning the arms
+    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n Press Button 1 to start positioning the arms using PotControl\r\n");
+    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
+    pc.printf("\r\n ***************** \r\n");
+    while (button1_value == false){}
+    PotControlTicker.attach(&PotControl,CONTROLLER_TS);
     
-    /**Attach the 'sample' function to the timer 'sample_timer'.
-    * this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
-    */
-   // sample_timer.attach(&sample, 0.002);
-
-    // ticker voor hoek berekenen aanzetten 
-    TickerCalculationsForTheta.attach(&CalculationsForTheta,1); //0.002 
-   
-    //empty loop, sample() is executed periodically
-    while(1) {
-        
-        }
+    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n When done positioning, press Button 1 to detach Potcontrol");
+    pc.printf("\r\n ***************** \r\n");
+    while (button1_value == true){}
+    
+    // Detach potcontrol
+    PotControlTicker.detach();
+    
+    // Set motors to 0
+    SetMotor(1,0.0);
+    SetMotor(2,0.0);
+    // Wait a bit to let the movement stop
+    wait(0.5);
+    // Reset the encoders to set the 0 position
+    ResetEncoders();
+    
+    // PID control starts
+    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n When done positioning, press button SW3 to start potmeter PID control");
+    pc.printf("\r\n Make sure both potentiometers are positioned halfway!!! \r\n");
+    pc.printf("\r\n ***************** \r\n");
+    while (button1_value == false){}
+    
+    // Attach sample, calc and control tickers
+    SampleTicker.attach(&sample, SAMPLE_TS);
+    CalculationsTicker.attach(&CalculationsForTheta,CALC_TS);
+    PIDControlTicker.attach(&Controller_motor, CONTROLLER_TS); // 100 Hz    
+    while(true){}
 }
\ No newline at end of file