File content as of revision 1:5acd27cb1857:

#include "mbed.h"
#include "HallDecoder.h"
#include "WiiNunchuck.h"

//------------------------------------
// Hyperterminal configuration
// 9600 bauds, 8-bit data, no parity
//------------------------------------

#define NELEMS(x)  (sizeof(x) / sizeof(x[0]))   // number of elements in array

#define DEBUG_SERIAL_SPEED  115200

#define NUNCHUCK_I2C_CLK    PA_15
#define NUNCHUCK_I2C_SDA    PA_14
#define NUNCHUCK_X_OFFSET   123
#define NUNCHUCK_Y_OFFSET   127

#define MOTOR_LEFT_TXD      PB_10           // SERIAL1
#define MOTOR_LEFT_RXD      NC
#define MOTOR_LEFT_HALL1    PB_15
#define MOTOR_LEFT_HALL2    PB_14
#define MOTOR_LEFT_HALL3    PB_6

#define MOTOR_RIGHT_TXD     PC_4            // SERIAL3
#define MOTOR_RIGHT_RXD     NC
#define MOTOR_RIGHT_HALL1   PC_7
#define MOTOR_RIGHT_HALL2   PA_8
#define MOTOR_RIGHT_HALL3   PA_9

#define HOVER_SPEED_MAX (90*2)              // ticks per second

/******************************************************************************/
WiiNunchuck m_oNunchuck(NUNCHUCK_I2C_SDA, NUNCHUCK_I2C_CLK);    // I2C1
Serial      m_oPc(SERIAL_TX, SERIAL_RX);     // PA_2, PA_3 -> SERIAL2
Timer       m_oTimer;
Ticker      m_oPeriodic;
DigitalOut  led1(LED1);

HallDecoder oHallLeft(MOTOR_LEFT_HALL1, MOTOR_LEFT_HALL2, MOTOR_LEFT_HALL3);
HallDecoder oHallRight(MOTOR_RIGHT_HALL1, MOTOR_RIGHT_HALL2, MOTOR_RIGHT_HALL3);

typedef struct t_controller {
    int16_t     w;          //< setpoint
    int16_t     u;          //< input for controler
    int16_t     y;          //< actual value
    int16_t     yfilt[3];
    int16_t     e;          //< error
    int16_t     eSum;       //< error sum
    float       kp;         //< controller setting
    float       ki;         //< controller setting
    float       kd;         //< controller setting
} t_controller;

#define LEFT    0
#define RIGHT   1
volatile t_controller _Controlloop[2]; 


#define MAX_ERROR   100

/******************************************************************************/
// periodic timed control stuff
void ticker()
{
    // get new inputs
    int nTicks[2]; 
    nTicks[LEFT] = oHallLeft.getticks();
    nTicks[RIGHT] = oHallRight.getticks();
    
    oHallLeft.resetticks();
    oHallRight.resetticks();
    
    // for both sides
    for(int i=0; i<NELEMS(_Controlloop); i++) {
        _Controlloop[i].yfilt[0] = _Controlloop[i].yfilt[1];
        _Controlloop[i].yfilt[1] = _Controlloop[i].yfilt[2];
        _Controlloop[i].yfilt[2] = nTicks[i];
        _Controlloop[i].y = (_Controlloop[i].yfilt[0]+_Controlloop[i].yfilt[1]+_Controlloop[i].yfilt[2])/3;  // low pass filter
        
        // some max value clipping
        if(abs(_Controlloop[i].y)>500) {
            _Controlloop[i].y = 0;
        }
    
        int16_t nError = _Controlloop[i].w - _Controlloop[i].y;
        if(nError>MAX_ERROR) {
            nError = MAX_ERROR;
        } else if(_Controlloop[i].w - _Controlloop[i].y<-MAX_ERROR) {
            nError = -MAX_ERROR;
        }
    
        // actual controller equation part
        _Controlloop[i].eSum += nError;
        if(_Controlloop[i].eSum>100 ) {
            _Controlloop[i].eSum = 100;
        }
        else if( _Controlloop[i].eSum<-100) {
            _Controlloop[i].eSum = -100;
        }
        
        _Controlloop[i].u = nError* _Controlloop[i].kp + _Controlloop[i].eSum * _Controlloop[i].ki;
    
        _Controlloop[i].e = nError;
    } 
    
    // reverse one wheel direction to get same rotation direction for positive inputs
    _Controlloop[LEFT].u = _Controlloop[LEFT].u * -1;

    led1 = !led1;
}

/******************************************************************************/
int main()
{
    m_oPc.baud(DEBUG_SERIAL_SPEED);
    
    m_oTimer.start();
    m_oPeriodic.attach(ticker, 0.5);
    
    RawSerial oMotorRight(MOTOR_RIGHT_TXD, NC);
    oMotorRight.baud(26315);
    oMotorRight.format(9, SerialBase::None, 1);

    RawSerial oMotorLeft(MOTOR_LEFT_TXD, NC);
    oMotorLeft.baud(26315);
    oMotorLeft.format(9, SerialBase::None, 1);

    int nControl = 1;

    for(int i=0; i<NELEMS(_Controlloop); i++) {
        _Controlloop[i].kp = 2.0; 
        _Controlloop[i].ki = 0.3; 
        _Controlloop[i].kd = 0; 
    } 

    m_oPc.printf("Hello Hoverboard !\n");

    wait(1);
    m_oPc.printf("SystemCoreClock = %d Hz\n", SystemCoreClock);

    char c = ' ';

    while(1) {
        m_oPc.printf("%d\n", c);
        if(c == ' ') {
            _Controlloop[LEFT].w =0;
        } else if(c == 'q') {
            _Controlloop[LEFT].w -= 10;
        } else if(c == 'w') {
            _Controlloop[LEFT].w += 10;
        } else if(c == '2') {
            _Controlloop[LEFT].w += 100;
        }  else if(c == '1') {
            _Controlloop[LEFT].w-= 100;
        } else if(c=='r') {
            m_oPc.printf("Reset ticks\n");
            oHallLeft.resetticks();
            oHallRight.resetticks();
        } else if(c=='n') {
            if(nControl==0) {
                m_oPc.printf("Change to nunchuck control");
                nControl = 1;
            } else {
                m_oPc.printf("Change to keyboard control");
                nControl = 0;
            }
        } else if(c=='m') {
            m_oPc.printf("nunchuck x: %d\n", m_oNunchuck.joyx());
            m_oPc.printf("nunchuck x: %d\n", m_oNunchuck.joyy());
            m_oPc.printf("nunchuck c: %d\n", m_oNunchuck.buttonc());
            m_oPc.printf("nunchuck z: %d\n", m_oNunchuck.buttonz());
        }

        m_oPc.printf("speed %d\n", _Controlloop[LEFT].w);

        // main control loop
        do {
            if(m_oTimer.read_ms() > 500) {
                m_oTimer.reset();
                m_oPc.printf("Speed set %d speed: %d speed out: %d error: %d \n", 
                    _Controlloop[LEFT].w, _Controlloop[LEFT].y, _Controlloop[LEFT].u, _Controlloop[LEFT].e);
                m_oPc.printf(" Speed set %d speed: %d speed out: %d error: %d \n", 
                    _Controlloop[RIGHT].w, _Controlloop[RIGHT].y, _Controlloop[RIGHT].u, _Controlloop[RIGHT].e);
            }

            if(nControl) {
                int nJoyY = ((m_oNunchuck.joyy()-NUNCHUCK_Y_OFFSET)*HOVER_SPEED_MAX) / 127; // + -> forward
                int nJoyX = ((m_oNunchuck.joyx()-NUNCHUCK_X_OFFSET)*HOVER_SPEED_MAX) / 127; // + -> right
                
                if(m_oNunchuck.buttonz()) {
                    // same inputs for both wheels
                    _Controlloop[LEFT].w  = nJoyY;
                    _Controlloop[RIGHT].w  = nJoyY;
                }
                else {
                    _Controlloop[LEFT].w  = nJoyY + nJoyX/2; 
                    _Controlloop[RIGHT].w = nJoyY - nJoyX/2; 
                }
                
                
                if(m_oNunchuck.buttonc()) {
                    m_oPc.printf("switch back to auto mode\n");
                    nControl = 0;   // fallback to manual controll
                }
            }

            oMotorRight.putc(256);
            oMotorRight.putc(_Controlloop[RIGHT].u & 0xFF);
            oMotorRight.putc((_Controlloop[RIGHT].u >> 8) & 0xFF);
            oMotorRight.putc(_Controlloop[RIGHT].u & 0xFF);
            oMotorRight.putc((_Controlloop[RIGHT].u >> 8) & 0xFF);
            oMotorRight.putc(85);
            oMotorRight.putc(82);
            oMotorRight.putc(82);

            wait_ms(1);

            oMotorLeft.putc(256);
            oMotorLeft.putc(_Controlloop[LEFT].u & 0xFF);
            oMotorLeft.putc((_Controlloop[LEFT].u >> 8) & 0xFF);
            oMotorLeft.putc(_Controlloop[LEFT].u & 0xFF);
            oMotorLeft.putc((_Controlloop[LEFT].u >> 8) & 0xFF);
            oMotorLeft.putc(85);
            oMotorLeft.putc(82);
            oMotorLeft.putc(82);

            
        } while(!m_oPc.readable());
        c=m_oPc.getc();
    }
}