File content as of revision 4:76b3113c79ff:

/*
 * mbed Application program for the mbed
 *      Test program for GR-PEACH
 *
 * Copyright (c) 2014 Kenji Arai / JH1PJL
 *  http://www.page.sannet.ne.jp/kenjia/index.html
 *  http://mbed.org/users/kenjiArai/
 *      Created: November  29th, 2014
 *      Revised: December  14th, 2014
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

//  Include ---------------------------------------------------------------------------------------
#include    "mbed.h"
#include    "rtos.h"
#include    "L3GD20.h"
#include    "LIS3DH.h"
#include    "ST7565_SPI_LCD.h"
#include    "PID.h"
#include    "stepper.h"

//  Definition ------------------------------------------------------------------------------------
#define USE_COM         // use Communication with PC(UART)
 
// Com
#ifdef  USE_COM
#define BAUD(x)             pcx.baud(x)
#define GETC(x)             pcx.getc(x)
#define PUTC(x)             pcx.putc(x)
#define PRINTF(...)         pcx.printf(__VA_ARGS__)
#define READABLE(x)         pcx.readable(x)
#else
#define BAUD(x)             {;}
#define GETC(x)             {;}
#define PUTC(x)             {;}
#define PRINTF(...)         {;}
#define READABLE(x)         {;}
#endif

#define TIMEBASE        12000
#define FIXED_STEPS     100

#define PI              3.1415926536
#define RAD_TO_DEG      57.29578
#define TIME_BASE_S     0.01
#define TIME_BASE_MS    ( TIME_BASE_S * 1000)
#define RATE            0.1

//  Object ----------------------------------------------------------------------------------------
// LED's
DigitalOut LEDs[4] = {
    DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)
};
// Swiches
DigitalIn   USER_SWITCH[2] = {
    #if defined(TARGET_RZ_A1H)
    DigitalIn(P6_0),  DigitalIn(P6_1)
    #elif defined(TARGET_NUCLEO_F401RE) || defined(TARGET_NUCLEO_F401RE)\
     || defined(TARGET_NUCLEO_L152RE)
    DigitalIn(PC_13),  DigitalIn(A0)
    #elif defined(TARGET_LPC1768)
    DigitalIn(A0),  DigitalIn(A1)
    #elif defined(TARGET_K64F)
    DigitalIn(PTA4),  DigitalIn(PTC6)
    #endif
};
// Rotor
STEPPER     rotor(D5, D4, D3, D2);
// com
#ifdef USE_COM
Serial      pcx(USBTX, USBRX);      // Communication with Host
#endif
I2C i2c(D14,D15);
// Gyro
L3GX_GYRO   gyro(i2c, L3GD20_V_CHIP_ADDR, L3GX_DR_95HZ, L3GX_BW_HI, L3GX_FS_250DPS);
// Acc
LIS3DH      acc(i2c, LIS3DH_G_CHIP_ADDR, LIS3DH_DR_NR_LP_50HZ, LIS3DH_FS_8G);
// SPI LCD
SPI         spi_lcd(D11, D12, D13); // mosi, miso, sck
ST7565      lcd1(spi_lcd, D8, D9, D7, ST7565::AQM1248A); // spi,reset,a0,ncs, LCD(Akizuki AQM1248A)
// Kc, Ti, Td, interval
PID         controller(1.0, 0.0, 0.0, RATE);
// Mutex
Mutex       i2c_mutex; 

//  RAM -------------------------------------------------------------------------------------------
Queue<uint32_t, 2> queue0;
Queue<uint32_t, 2> queue1;
float fa[3];    // Acc  0:X, 1:Y, 2:Z
float fg[3];    // Gyro 0:X, 1:Y, 2:Z
float accXangle;    // Angle calculate using the accelerometer
float gyroXangle;   // Angle calculate using the gyro
float kalAngleX;    // Calculate the angle using a Kalman filter
float stp;
float angle;

uint8_t pls_width[MT_SLOP_STEP] = {5, 4, 3, 2, 1, 1, 1, 1, 1, 1 };

/* Mail */
typedef struct {
  float    voltage; /* AD result of measured voltage */
  float    current; /* AD result of measured current */
  uint32_t counter; /* A counter value               */
} mail_t;
 
Mail<mail_t, 16> mail_box;

uint8_t show_flag;

//  ROM / Constant data ---------------------------------------------------------------------------

//  Function prototypes ---------------------------------------------------------------------------

//  Function prototypes ---------------------------------------------------------------------------
extern int mon( void);
extern float kalmanCalculate(float newAngle, float newRate, int looptime);

//-------------------------------------------------------------------------------------------------
//  Control Program
//-------------------------------------------------------------------------------------------------
void send_thread (void const *args) {
    uint32_t i = 0;
    while (true) {
        i++; // fake data update
        mail_t *mail = mail_box.alloc();
        mail->voltage = (i * 0.1) * 33; 
        mail->current = (i * 0.1) * 11;
        mail->counter = i;
        mail_box.put(mail);
        Thread::wait(1000);
    }
}

void blink(void const *n) {
    LEDs[(int)n] = !LEDs[(int)n];
}

// Read switch status
int read_sw(uint8_t n){
    if (USER_SWITCH[n] == 0){   return 1;
    } else {                    return 0;}
}

// Monitor program
void monitor(void const *args){
    while (true){
        mon();
    }
}

// Interrupt routine
void queue_isr0() {
    queue0.put((uint32_t*)1);
}

void queue_isr1() {
    queue1.put((uint32_t*)1);
}

// Update sensor data
void update_angle(void const *args){
    while (true) {
        osEvent evt = queue0.get();
        // ---->lock
        i2c_mutex.lock();
        // read acceleration data from sensor
        acc.read_data(fa);
        // read gyroscope data from sensor
        gyro.read_data(fg);
        // <----unlock
        i2c_mutex.unlock();
        // Calculate angle (degree)
        accXangle = (atan2(-fa[1],fa[2])+PI)*RAD_TO_DEG;
        // calculate filtered Angle
        kalAngleX = kalmanCalculate(accXangle, fg[0], TIME_BASE_MS) - 180;
    }
}

// Read angle and control an inertia rotor
void rotor_control(void const *args){
    // Input angle range
    controller.setInputLimits(-90.0, 90.0);
    // Output motor speed
    controller.setOutputLimits(-50, 50);
    // a bias.
    controller.setBias(0.0);
    controller.setMode(AUTO_MODE);
    // Target
    controller.setSetPoint(0.0);
    while (true) {
        osEvent evt = queue1.get();
        // Update the process variable.
        if ((kalAngleX < 0.8) && (kalAngleX > -0.8)){
            angle = 0;
        } else {
            angle = kalAngleX;
        }
        controller.setProcessValue(angle);
        // Set the new output.
        stp = controller.compute() * 5;
        rotor.move((int32_t)stp);
    }
}

// Update sensor data
void display(void const *args){
    // SPI LCD
    spi_lcd.frequency(100000);
    lcd1.cls();
    lcd1.set_contrast(0x2a);
    lcd1.printf("test\r\n" );
    lcd1.printf("Kenji Arai / JH1PJL\r\n" );
    lcd1.printf("ABCDEFG 1234567890\r\n" );
    lcd1.rect(5,30,120,62,1);
    lcd1.circle(5,35,5,1);
    lcd1.fillcircle(60,55,5,1);
    lcd1.line(0,30,127,63,1);
    while (true) {
        Thread::wait(500);
    }
}

// Thread definition
osThreadDef(update_angle, osPriorityRealtime, 4096);
osThreadDef(rotor_control, osPriorityAboveNormal, 4096);
osThreadDef(monitor, osPriorityNormal, 4096);
osThreadDef(display, osPriorityNormal, 4096);

int main(void) {
    PRINTF("step1\r\n");
    
    RtosTimer led_1_timer(blink, osTimerPeriodic, (void *)0);
    RtosTimer led_2_timer(blink, osTimerPeriodic, (void *)1);
    RtosTimer led_3_timer(blink, osTimerPeriodic, (void *)2);
    RtosTimer led_4_timer(blink, osTimerPeriodic, (void *)3);

    PRINTF("step2\r\n");    
    led_1_timer.start(2000);
    led_2_timer.start(1000);
    led_3_timer.start(500);
    led_4_timer.start(250);

    PRINTF("step3\r\n");    
    Thread thread(send_thread);

    PRINTF("step4\r\n");
    // Initialize data
    stp = 0;
    angle = 0.0;

    // IRQ
    Ticker ticker0;
    Ticker ticker1;
    ticker0.attach(queue_isr0, TIME_BASE_S);
    ticker1.attach(queue_isr1, RATE);
    rotor.set_max_speed(TIMEBASE);
    
    PRINTF("step5\r\n");
    // Starts 1st thread
    osThreadCreate(osThread(update_angle), NULL);
    // Starts 2nd thread
    osThreadCreate(osThread(rotor_control), NULL);
    // Starts 3rd thread
    osThreadCreate(osThread(monitor), NULL);
    // Starts 4th thread
    osThreadCreate(osThread(display), NULL);

    PRINTF("step6\r\n");   
    while (true) {
        osEvent evt = mail_box.get();
        if (evt.status == osEventMail) {
            mail_t *mail = (mail_t*)evt.value.p;
            if (show_flag){
                PRINTF("This is dummy!, ");
                PRINTF("Volt: %.2f V, "   , mail->voltage);
                PRINTF("Current: %.2f A, "     , mail->current);
                PRINTF("# of cycles: %u\r\n", mail->counter);
            }
            mail_box.free(mail);
        }
    }
}