File content as of revision 0:b4a14139fab8:

#include "mbed.h"
#include "SP_FP.h"
#include "PID.h"

#define RATE 0.01
#define AUTO 1

PID controller(0.6, 0.0, 0.4, RATE); // Kc, Ti, Td, interval
SPI yei(p11, p12, p13); // mosi, miso, sclk
DigitalOut yei_cs(p14); // chip sellect
SP_FP sp_fp; // Single precision floating point
SPI amis30543(p5, p6, p7); // mosi, miso, sclk
DigitalOut cs(p8); // chip sellect
DigitalOut nextStep(p9); // Step move
DigitalOut direction(p10); // Direction
Serial pc(USBTX, USBRX); //  Used for printf to PC over USB
// Delays are in MicroSeconds
float stepDelay = 20.0f;
float directionDelay = 100.0f;
float registerDelay = 5.0f;
float tempRead = 0.0f;
float tempCompute = 0.0f;
uint8_t tempByte = 0;
uint16_t readPos = 0;

void writeReg(uint8_t address, uint8_t value) {
    cs = 0;
    amis30543.write(0x80 | (address & 0b11111)); // I do not know why :S, Josh explained why anyway
    amis30543.write(value);
    cs = 1;
    wait_us(registerDelay);
}

void step(void) {
  nextStep = 1;
  wait_us(stepDelay);
  nextStep = 0;
  wait_us(stepDelay);
}

void changeDir(int dir) {
    wait_us(directionDelay);
    if(dir == 0) {
        writeReg(0x02, 0x80); // CR1 - Change Direction
    }
    else {
        writeReg(0x02, 0x00); // CR1 - Change Direction
    }
    wait_us(directionDelay);
}

uint8_t readReg(uint8_t address) {
    cs = 0;
    tempByte = amis30543.write(address & 0b11111);
    cs = 1;
    return tempByte;
}

/*! Reads a status register and returns the lower 7 bits (the parity bit is
 *  set to 0 in the return value). */
uint8_t readStatusReg(uint8_t address) {
    // Mask off the parity bit.
    // (Later we might add code here to check the parity
    // bit and record errors.)
    return readReg(address) & 0x7F;
}

uint16_t readPosition() {
    uint8_t sr3 = readStatusReg(0x07); // SR3 0x07
    uint8_t sr4 = readStatusReg(0x0A); // SR4 0x0A
    return ((uint16_t)sr3 << 2) | (sr4 & 3);
}

float readPitch() {
    int i = 0;
    uint8_t value[13] = {0};
    uint32_t pyr[3] = {0};
    float yaw = 0.0f, pitch = 0.0f, roll = 0.0f, tempCheck = 0.0f;
    // Select the device by seting chip select low
        yei_cs = 0;

        // Send 0xF6, to start communication on SPI
        value[0] = yei.write(0xF6);
        //printf("F6 is sent and 00 is waiting. The return result is:  0x%X\n", value[0]);

        // Send 0x07, to request untared euler angles
        value[0] = yei.write(0x07);
        //printf("07 is sent and 04 is waiting. The return result is:  0x%X\n", value[0]);

        for(i=0; i<=12; i++) {
            value[i] = yei.write(0xFF);
            if(value[i] == 1) {
                i = 0;
            }
            wait(0.01);
        }

        pyr[0] = value[1];
        pyr[0] = (pyr[0] << 8) + value[2];
        pyr[0] = (pyr[0] << 8) + value[3];
        pyr[0] = (pyr[0] << 8) + value[4];
        tempCheck = sp_fp.decoder(pyr[0]) * 180.0f / 3.1415926535897932384626433832795;
        if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
           pitch = tempCheck;
        pyr[1] = value[5];
        pyr[1] = (pyr[1] << 8) + value[6];
        pyr[1] = (pyr[1] << 8) + value[7];
        pyr[1] = (pyr[1] << 8) + value[8];
        tempCheck = sp_fp.decoder(pyr[1]) * 180.0f / 3.1415926535897932384626433832795;
        if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
            yaw = tempCheck;
        pyr[2] = value[9];
        pyr[2] = (pyr[2] << 8) + value[10];
        pyr[2] = (pyr[2] << 8) + value[11];
        pyr[2] = (pyr[2] << 8) + value[12];
        tempCheck = sp_fp.decoder(pyr[2]) * 180.0f / 3.1415926535897932384626433832795;
        if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
            roll = tempCheck;

        pc.printf("Pitch: %f     Yaw: %f     Roll: %f\n", pitch, yaw, roll);

        // Deselect the device
        yei_cs = 1;
        return pitch;
}

int main() {
    yei_cs = 1;
    yei.format(8,0);
    yei.frequency(1000000);
    pc.baud(9600);
    controller.setInputLimits(-10.0, 10.0);
    controller.setOutputLimits(-1000, 1000);
    controller.setBias(0.0001);
    controller.setMode(AUTO);
    controller.setSetPoint(-3.2);
    int i = 0;
    wait(1);

    writeReg(0x03, 0x80); // CR2
    writeReg(0x00, 0x00); // WR
    // for CR0: 132mA->X0, 485mA->X5, 955mA-> XC 
    writeReg(0x01, 0x00); // CR0
    writeReg(0x02, 0x80); // CR1
    writeReg(0x09, 0x01); // CR3

    wait_us(registerDelay);
    while(1) {
        tempRead = readPitch();tempRead+=4.0f;
        //controller.setProcessValue(tempRead);
        //tempCompute = controller.compute();
        //pc.printf("PID:%f \t pitch:%f\n", tempCompute, tempRead);
        wait(RATE);
        if(tempRead < 0) {
            changeDir(0);
            step();
            /*for(i=0; i>=tempCompute; i--) {
                step();
            }*/
        }
        else {
            changeDir(1);
            step();
            /*for(i=0; i<=tempCompute; i++) {
                step();
            }*/
        }
    }
}