File content as of revision 1:47a1aaf98baa:

//  Copyright 2019 Tsung-Han Lee, National Tsing Hua Univeristy Dynamic System and Control Lab, Tommy Cheng, Skyler Chen
/***************************************************************
 **     MOBILE ROBOT LAB2 GROUP 4 [Vehicle Motion Control]
 **
 **  - Members  : Tsung-Han Lee, Tommy, Skyler
 **  - NOTE     : Modified from sample code provided
 **               by NTHU Dynamic Systems and Control Lab
 ** 
 ***************************************************************/
#include "mbed.h"
#include "hallsensor_software_decoder.h"

#define Ts 0.01

Ticker controllerTimer;

/* declare pwm pin */
PwmOut pwm1(D7);
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);

/********************* Helper Functions and Structures *******************/
typedef volatile struct Controller {
    volatile double rpm;        // rotation speed in rpm
    volatile double piOut;      // PI Controller output
    volatile double err;        // error
    volatile double ierr;       // integration of error
    double kp;                  // propotinal gain
    double ki;                  // integrational gain
} Controller_t;

/* Init Structure Instance */
Controller_t controller1 = {0.0, 0.0, 0.0, 0.0, 0.008, 0.02};
Controller_t controller2 = {0.0, 0.0, 0.0, 0.0, 0.008, 0.02};

volatile double refRPM1  = 25.0;      // reference rpm1
volatile double refRPM2  = -25.0;      // reference rpm2


void initTimer();
void initPWM();
void controllerTISR();
/************************************************************************/

/* SERIAL DEBUG */
//Serial pc(USBTX, USBRX);

int main(int argc, char* argv[]) {
    /* SERIAL DEBUG */
//    pc.baud(115200);
    
    initTimer();
    initPWM();
    init_CN();
    
    while (1) {
//        pc.printf("%.2f, %.2f \r\n", controller1.rpm, controller2.rpm);
        wait_ms(100);  
    }
}

void initTimer() {
    controllerTimer.attach_us(&controllerTISR, 10000.0);    
}

void initPWM() {
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
    
    pwm2.period_us(50);
    pwm2.write(0.5);
    TIM1->CCER |= 0x40;   
}

void controllerTISR() {        
    /********* MOTOR 1 ******************/
    // 100/48*60/56 = 2.232142857
    controller1.rpm            = (double)wheelState1.numStateChange * 2.232142857;
    wheelState1.numStateChange = 0;
    
    controller1.err   = refRPM1 - controller1.rpm;
    controller1.ierr += Ts*controller1.err;
    
    if (controller1.ierr > 50.0) {
        controller1.ierr = 50.0;    
    } else if (controller1.ierr < -50.0) {
        controller1.ierr = -50.0;    
    }
    controller1.piOut = controller1.kp*controller1.err + controller1.ki*controller1.ierr;

    if (controller1.piOut >= 0.5) {
        controller1.piOut = 0.5;    
    } else if (controller1.piOut <= -0.5) {
        controller1.piOut = -0.5;    
    }
    
    pwm1.write(controller1.piOut + 0.5);
    TIM1->CCER |= 0x4;
    
    /********* MOTOR 2 ******************/
    // 100/48*60/56 = 2.232142857
    controller2.rpm            = (double)wheelState2.numStateChange * 2.232142857;
    wheelState2.numStateChange = 0;
    
    controller2.err   = refRPM2 - controller2.rpm;
    controller2.ierr += Ts*controller2.err;
    
    if (controller2.ierr > 50.0) {
        controller2.ierr = 50.0;    
    } else if (controller2.ierr < -50.0) {
        controller2.ierr = -50.0;    
    }
    controller2.piOut = controller2.kp*controller2.err + controller2.ki*controller2.ierr;

    if (controller2.piOut >= 0.5) {
        controller2.piOut = 0.5;    
    } else if (controller2.piOut <= -0.5) {
        controller2.piOut = -0.5;    
    }
    
    pwm2.write(-controller2.piOut + 0.5);
    TIM1->CCER |= 0x40;
}