File content as of revision 1:fbed80ea9cf9:

#include "mbed.h"

// Sampling frequency
#define SAMP_FREQ 2000.0
// Motor voltage limit (V_motor = 3.3 * v_out * 4)
#define VLIMIT 0.1
// Motor offset voltage
#define VOFFSET 0.0
// Feedback gains (Arbitrary Unit)
#define KP 1.0
#define KD 0.04
#define KX -0.001
#define KV -0.0007


// Define LEDs
PwmOut led_blink(LED1);             // Blink at 1 Hz during operation
PwmOut led_v_over(LED2);            // Indicate voltage saturation
PwmOut led_v_forward(LED3);         // Indicate voltage for forward motion
PwmOut led_v_backward(LED4);        // Indicate voltage for backward motion

// Define motor controls
DigitalOut motor_f(p13);            // Motor forward
DigitalOut motor_r(p14);            // Motor backward
AnalogOut motor_v(p18);             // Motor driving voltage

// Define Photo-diode readings for angle sensor
AnalogIn pd1(p19);
AnalogIn pd2(p20);

// Define Photo-diode readings for encoder
AnalogIn pd3(p15);
AnalogIn pd4(p16);

Serial pc(USBTX, USBRX);            // Serial communication
Ticker t_int;                       // Interrupt ticker

float brightness = 0;               // Brightness for blinking led_blink
float br_inc = 1.0/SAMP_FREQ;       // For blinkng LED at 1 Hz
float angle, angle_prev = 0;        // Angle and Previous angle (for calculating omega)
float v_out;                        // Output voltage
float v_dir;                        // Voltage polarity
float omega = 0;                    // Angluar velocity
float f_omega = 0;                  // Filtered angluar velocity
int value1 = 0;
int value2 = 0;
int value1_prev = 0;
int value2_prev = 0;
int count = 0;
int count_prev = 0;
float velocity = 0;
float velocity_prev = 0;
float velocity_prev_prev = 0;
float f_velocity = 0;
float f_velocity_prev = 0;
float f_velocity_prev_prev = 0;
float omega_prev = 0;
float f_omega_prev = 0;
float ref = 0;

// Interrupt handler (control loop)
void int0() {
    // read difference of photo-diodes
    angle = pd1 - pd2;

    //binal
    if (pd3 > 0.65) {
        value1  = 1;
    } else {
        value1  = 0;
    }
    if (pd4 > 0.72) {
        value2  = 1;
    } else {
        value2  = 0;
    }
    //keisoku
    if(value1 != value1_prev){
        //rising
        if(value1 == 1){
            if(value2 == 1){
                count += 1;
            } else {
                count -= 1;
            }
        //falling
        } else {
            if(value2 ==1){
                count -= 1;
            } else {
                count += 1;
            }
        }
    }
    if(value2 != value2_prev){
        //rising
        if(value2 == 1){
            if(value1 == 1){
                count -= 1;
            } else {
                count += 1;
            }
        //falling
        } else {
            if(value1 ==1){
                count += 1;
            } else {
                count -= 1;
            }
        }
    }

    // calculate omega
    omega = (angle - angle_prev) * SAMP_FREQ;
    angle_prev = angle;
    // filtering omega
    f_omega = 0.0155*omega + 0.0155*omega_prev + 0.9691*f_omega_prev;
    omega_prev = omega;
    f_omega_prev = f_omega;

    // calculate velocity
    velocity = (count - count_prev) * SAMP_FREQ;
    // filtering velocity
    f_velocity = 0.0002414*velocity + 0.0004827*velocity_prev + 0.0002414*velocity_prev_prev + 1.9556*f_velocity_prev - 0.9565*f_velocity_prev_prev;

    //update value
    velocity_prev_prev = velocity_prev;
    velocity_prev = velocity;
    
    f_velocity_prev_prev = f_velocity_prev;
    f_velocity_prev = f_velocity;

    count_prev = count;
    value1_prev = value1;
    value2_prev = value2;  

    
    // output
    v_out = angle * KP + f_omega * KD + (count - ref) * KX + f_velocity * KV;
    ref += 40.0 / 2000.0;
    //v_out = count * KX + f_velocity * KV;
    //v_out = angle * KP + f_omega * KD ;
    if (v_out > 0) {
        motor_f = 1;
        motor_r = 0;
        v_dir = 1.0;
    } else {
        motor_f = 0;
        motor_r = 1;
        v_out *= -1.0;
        v_dir = -1.0;
    }
    if (v_out > VLIMIT) {
        v_out = VLIMIT;
        led_v_over = 1;
    } else {
        led_v_over = 0;
    }
    // Motor output
    if (pd1 < 0.02 && pd2 < 0.02) {     // if not standing, stop the motor
        motor_v = 0;
        led_v_forward = 1;
        led_v_backward = 1;
    } else {
        motor_v = v_out + VOFFSET;
        led_v_forward = (v_dir > 0 ? v_out*3.3 : 0);
        led_v_backward = (v_dir < 0 ? v_out*3.3 : 0);
    }
    // blink LED
    brightness += br_inc;
    if (brightness >= 1) brightness = 0;
    led_blink = brightness;

    // send 6 data to PC (data should be in [0,255]. Data must be casted into (char) type)
    //pc.putc((char)(pd1 * 128 + 128));
    //pc.putc((char)(pd2 * 128 + 128));
    pc.putc((char)(f_velocity + 128));
    pc.putc((char)(count + 128));
    pc.putc((char)(velocity + 128));
    //pc.putc((char)(angle * 128 + 128));
    //pc.putc((char)(value1 * 128 + 100));
    //pc.putc((char)(value2 * 128 + 120));
    //pc.putc((char)(omega * 10 + 128));
    //pc.putc((char)(f_omega * 10 + 128));
    pc.putc((char)(count + 128));
    pc.putc((char)(f_velocity + 128));
    pc.putc((char)(v_out * 100 + 128));

}


// Main function is only for initialization
int main() {
    // Initializing LED ports and Serial port
    led_blink.period(1.0/SAMP_FREQ);
    led_v_over.period(1.0/SAMP_FREQ);
    led_v_forward.period(1.0/SAMP_FREQ);
    led_v_backward.period(1.0/SAMP_FREQ);
    
    pc.baud(230400);

    // Wait for 1 second before starting feedback control
    wait(1);
    pc.printf("12345678");                 // For initializing Processing plotter program

    // Start Control (to be stopped with any data from serial port)
    t_int.attach(&int0, 1.0/SAMP_FREQ);    // Start timer-interrupt
    while(1) {
        if (pc.readable() == 1) {          // If any data in serial prot
            pc.putc(pc.getc());            // Echo back the data
            t_int.detach();                // Stop timer-interrupt
            break;
        }
    }
}