LORDTEK LORDTEK
/
AMIS30543
AMIS 30543 Basic
main.cpp
- Committer:
- LORDTEK
- Date:
- 2016-08-16
- Revision:
- 0:b4a14139fab8
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(); }*/ } } }