File content as of revision 7:b881681f7ff6:

#include "mbed.h"
#include "FastPWM.h"
#include "AutomationElements.h"

DigitalIn fault(PTE20);
DigitalIn sw1(PTC13);
DigitalIn sw2(PTC17);
AnalogIn pte23(PTE23); //bugirani analogini ulaz, zamjena PTE30
AnalogIn a_feedback(PTE30);
AnalogIn b_feedback(PTE22);
AnalogIn pot1(PTB3);
AnalogIn pot2(PTB2);
AnalogIn camera(PTD5);
AnalogIn naponBaterije(PTE29);

Serial pc(PTA2, PTA1);

DigitalOut enable(PTE21);  
DigitalOut d1(PTB8); 
DigitalOut d2(PTB9);
DigitalOut d3(PTB10);
DigitalOut d4(PTB11);
//DigitalOut red(PTB18);
//DigitalOut green(PTB19);
//DigitalOut blue(PTD1);
/**
DigiralOut SI(PTD7); // camera SI

PwmOut cameraCLK(PTE1);
**/
PwmOut servo(PTB0);

FastPWM motorA(PTC3);
DigitalOut motorA_grana2(PTC4);
FastPWM motorB(PTC1);
DigitalOut motorB_grana2(PTC2);
FastPWM test_w(PTA12);

Ticker ticker;
Ticker tikref;
//Ticker cameraSI;

float u_Ri_a = 0, u_Ri_b = 0, n_ref=0, Ubat = 0, R_au = 1.067, L_au = 218e-6, K = 2.54e-3, pot2_skalirani = 0;
float u_i_a2 = 0, u_i_b2 = 0, w_a = 0, w_b = 0, N = 7.46, n_a = 0, n_b = 0, delay = 0.13, u_Rw_a = 0, u_Rw_b =0;
double u_i_a = 0, u_i_b = 0, e_a = 0, e_b = 0;
double T_d = 1e-3;

PI PI_a (0.04, 0.001, T_d);
PI PI_b (0.04, 0.001, T_d);

PI RegulatorBrzine_a( 0.01, 0.01, T_d);
PI RegulatorBrzine_b( 0.01, 0.01, T_d);

Timer timer;


void tick(){
    timer.start();
    Ubat = naponBaterije * 3.3 * (57.0/10.0);
    timer.stop();
    test_w.pulsewidth_us(timer.read_us());
    timer.reset();    
    if ((u_Ri_a - delay) <= 0) u_i_a = 0;
    else u_i_a = 375.0/220.0*(3.3 * a_feedback)/(u_Ri_a - delay);
    if ((u_Ri_b - delay) <= 0) u_i_b = 0;
    else u_i_b = 375.0/220.0*(3.3 * b_feedback)/(u_Ri_b - delay);
    //pot2_skalirani = 100 + 1900* pot2;
    //x = pot2_skalirani;
    //x = 3000 * pot2;
    n_ref = 1000;
    
                
    e_a = Ubat*(u_Ri_a - delay) - R_au*u_i_a - L_au * ((u_i_a - u_i_a2)/T_d);
    w_a = e_a / K;
    n_a = w_a * (30/3.14159) / N;
    u_i_a2 = u_i_a;
    e_b = Ubat*(u_Ri_b - delay) - R_au*u_i_b - L_au * ((u_i_b - u_i_b2)/T_d);
    w_b = e_b / K;
    n_b = w_b * (30/3.14159) / N;
    u_i_b2 = u_i_b;
    
    RegulatorBrzine_a.in(n_ref - n_a);
    RegulatorBrzine_b.in(n_ref - n_b);
    
    u_Rw_a = RegulatorBrzine_a.out();
    u_Rw_b = RegulatorBrzine_b.out();
            
    PI_a.in(u_Rw_a - u_i_a);
    PI_b.in(u_Rw_b - u_i_b);
    u_Ri_a = PI_a.out();
    u_Ri_b = PI_b.out();
      
    motorA.pulsewidth(100e-6*u_Ri_a);
    motorB.pulsewidth(100e-6*u_Ri_b);

        
    }
/**    
void camSI(){
    cameraCLK.pulsewidth(1.0/100e3 * 0.5);
    if (cameraCLK = 1) SI = 1
    
    }
**/
void steerServo(float angleDegrees){   
    float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
    if (angleDegrees < -29 ) angleDegrees = -29;
    if (angleDegrees > 30 ) angleDegrees = 30;
    pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
    servo.pulsewidth(pulse);
    }

int main() {
    float PWmin=-90, PWmax=90, angleDegrees = 0;
   
 //   cameraCLK.period(1.0/100e3); // frekvencija 100 kHz

    //servo.period(1.0/50);
    //steerServo(angleDegrees);
    
    PI_a.setOutputLimits (0, 1);
    PI_b.setOutputLimits (0, 1);
    
    RegulatorBrzine_a.setOutputLimits (0,3);
    RegulatorBrzine_b.setOutputLimits(0,3);
    
    motorA.period_us(100);
    motorB.period_us(100);
    test_w.period_us(10000);
    
    wait(3);
    enable = 1;
    motorB_grana2 = 0;
    motorA_grana2 = 0;
    ticker.attach(&tick, T_d);
    //tikref.attach(&changeRef,1);
//    cameraSI.attach(&camSI, 0.00258);  
float analogprint, analogprintb;
    while(1){
        if(sw1 == 1) enable = 0;
        if(sw2 == 1) enable = 1;
        if(fault == 1) {
            d1=1;
            d3=0;
            }
        else {
            d3=1;
            d1=0;
            }
/**       
        angleDegrees = PWmin + (PWmax - PWmin) * pot1;
        steerServo(angleDegrees);
**/

      
        
        wait(1);
        
        

analogprint = a_feedback;
analogprintb = b_feedback;
        pc.printf("a_feedback: %f b_feedback: %f \n\r",analogprint, analogprintb);

        }
}