main.cpp

00001 #include "mbed.h"
00002 #include "SP_FP.h"
00003 #include "PID.h"
00004 
00005 #define RATE 0.01
00006 #define AUTO 1
00007 
00008 PID controller(0.6, 0.0, 0.4, RATE); // Kc, Ti, Td, interval
00009 SPI yei(p11, p12, p13); // mosi, miso, sclk
00010 DigitalOut yei_cs(p14); // chip sellect
00011 SP_FP sp_fp; // Single precision floating point
00012 SPI amis30543(p5, p6, p7); // mosi, miso, sclk
00013 DigitalOut cs(p8); // chip sellect
00014 DigitalOut nextStep(p9); // Step move
00015 DigitalOut direction(p10); // Direction
00016 Serial pc(USBTX, USBRX); //  Used for printf to PC over USB
00017 // Delays are in MicroSeconds
00018 float stepDelay = 20.0f;
00019 float directionDelay = 100.0f;
00020 float registerDelay = 5.0f;
00021 float tempRead = 0.0f;
00022 float tempCompute = 0.0f;
00023 uint8_t tempByte = 0;
00024 uint16_t readPos = 0;
00025 
00026 void writeReg(uint8_t address, uint8_t value) {
00027     cs = 0;
00028     amis30543.write(0x80 | (address & 0b11111)); // I do not know why :S, Josh explained why anyway
00029     amis30543.write(value);
00030     cs = 1;
00031     wait_us(registerDelay);
00032 }
00033 
00034 void step(void) {
00035   nextStep = 1;
00036   wait_us(stepDelay);
00037   nextStep = 0;
00038   wait_us(stepDelay);
00039 }
00040 
00041 void changeDir(int dir) {
00042     wait_us(directionDelay);
00043     if(dir == 0) {
00044         writeReg(0x02, 0x80); // CR1 - Change Direction
00045     }
00046     else {
00047         writeReg(0x02, 0x00); // CR1 - Change Direction
00048     }
00049     wait_us(directionDelay);
00050 }
00051 
00052 uint8_t readReg(uint8_t address) {
00053     cs = 0;
00054     tempByte = amis30543.write(address & 0b11111);
00055     cs = 1;
00056     return tempByte;
00057 }
00058 
00059 /*! Reads a status register and returns the lower 7 bits (the parity bit is
00060  *  set to 0 in the return value). */
00061 uint8_t readStatusReg(uint8_t address) {
00062     // Mask off the parity bit.
00063     // (Later we might add code here to check the parity
00064     // bit and record errors.)
00065     return readReg(address) & 0x7F;
00066 }
00067 
00068 uint16_t readPosition() {
00069     uint8_t sr3 = readStatusReg(0x07); // SR3 0x07
00070     uint8_t sr4 = readStatusReg(0x0A); // SR4 0x0A
00071     return ((uint16_t)sr3 << 2) | (sr4 & 3);
00072 }
00073 
00074 float readPitch() {
00075     int i = 0;
00076     uint8_t value[13] = {0};
00077     uint32_t pyr[3] = {0};
00078     float yaw = 0.0f, pitch = 0.0f, roll = 0.0f, tempCheck = 0.0f;
00079     // Select the device by seting chip select low
00080         yei_cs = 0;
00081 
00082         // Send 0xF6, to start communication on SPI
00083         value[0] = yei.write(0xF6);
00084         //printf("F6 is sent and 00 is waiting. The return result is:  0x%X\n", value[0]);
00085 
00086         // Send 0x07, to request untared euler angles
00087         value[0] = yei.write(0x07);
00088         //printf("07 is sent and 04 is waiting. The return result is:  0x%X\n", value[0]);
00089 
00090         for(i=0; i<=12; i++) {
00091             value[i] = yei.write(0xFF);
00092             if(value[i] == 1) {
00093                 i = 0;
00094             }
00095             wait(0.01);
00096         }
00097 
00098         pyr[0] = value[1];
00099         pyr[0] = (pyr[0] << 8) + value[2];
00100         pyr[0] = (pyr[0] << 8) + value[3];
00101         pyr[0] = (pyr[0] << 8) + value[4];
00102         tempCheck = sp_fp.decoder(pyr[0]) * 180.0f / 3.1415926535897932384626433832795;
00103         if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
00104            pitch = tempCheck;
00105         pyr[1] = value[5];
00106         pyr[1] = (pyr[1] << 8) + value[6];
00107         pyr[1] = (pyr[1] << 8) + value[7];
00108         pyr[1] = (pyr[1] << 8) + value[8];
00109         tempCheck = sp_fp.decoder(pyr[1]) * 180.0f / 3.1415926535897932384626433832795;
00110         if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
00111             yaw = tempCheck;
00112         pyr[2] = value[9];
00113         pyr[2] = (pyr[2] << 8) + value[10];
00114         pyr[2] = (pyr[2] << 8) + value[11];
00115         pyr[2] = (pyr[2] << 8) + value[12];
00116         tempCheck = sp_fp.decoder(pyr[2]) * 180.0f / 3.1415926535897932384626433832795;
00117         if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
00118             roll = tempCheck;
00119 
00120         pc.printf("Pitch: %f     Yaw: %f     Roll: %f\n", pitch, yaw, roll);
00121 
00122         // Deselect the device
00123         yei_cs = 1;
00124         return pitch;
00125 }
00126 
00127 int main() {
00128     yei_cs = 1;
00129     yei.format(8,0);
00130     yei.frequency(1000000);
00131     pc.baud(9600);
00132     controller.setInputLimits(-10.0, 10.0);
00133     controller.setOutputLimits(-1000, 1000);
00134     controller.setBias(0.0001);
00135     controller.setMode(AUTO);
00136     controller.setSetPoint(-3.2);
00137     int i = 0;
00138     wait(1);
00139 
00140     writeReg(0x03, 0x80); // CR2
00141     writeReg(0x00, 0x00); // WR
00142     // for CR0: 132mA->X0, 485mA->X5, 955mA-> XC 
00143     writeReg(0x01, 0x00); // CR0
00144     writeReg(0x02, 0x80); // CR1
00145     writeReg(0x09, 0x01); // CR3
00146 
00147     wait_us(registerDelay);
00148     while(1) {
00149         tempRead = readPitch();tempRead+=4.0f;
00150         //controller.setProcessValue(tempRead);
00151         //tempCompute = controller.compute();
00152         //pc.printf("PID:%f \t pitch:%f\n", tempCompute, tempRead);
00153         wait(RATE);
00154         if(tempRead < 0) {
00155             changeDir(0);
00156             step();
00157             /*for(i=0; i>=tempCompute; i--) {
00158                 step();
00159             }*/
00160         }
00161         else {
00162             changeDir(1);
00163             step();
00164             /*for(i=0; i<=tempCompute; i++) {
00165                 step();
00166             }*/
00167         }
00168     }
00169 }