LORDTEK LORDTEK / Mbed 2 deprecated AMIS30543

AMIS 30543 Basic

Dependencies:   PID SP_FP mbed

main.cpp@0:b4a14139fab8, 2016-08-16 (annotated)

Committer:
LORDTEK
Date:
Tue Aug 16 08:20:04 2016 +0000
Revision:
0:b4a14139fab8
AMIS 30543 Basic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LORDTEK 0:b4a14139fab8 1 #include "mbed.h"
LORDTEK 0:b4a14139fab8 2 #include "SP_FP.h"
LORDTEK 0:b4a14139fab8 3 #include "PID.h"
LORDTEK 0:b4a14139fab8 4
LORDTEK 0:b4a14139fab8 5 #define RATE 0.01
LORDTEK 0:b4a14139fab8 6 #define AUTO 1
LORDTEK 0:b4a14139fab8 7
LORDTEK 0:b4a14139fab8 8 PID controller(0.6, 0.0, 0.4, RATE); // Kc, Ti, Td, interval
LORDTEK 0:b4a14139fab8 9 SPI yei(p11, p12, p13); // mosi, miso, sclk
LORDTEK 0:b4a14139fab8 10 DigitalOut yei_cs(p14); // chip sellect
LORDTEK 0:b4a14139fab8 11 SP_FP sp_fp; // Single precision floating point
LORDTEK 0:b4a14139fab8 12 SPI amis30543(p5, p6, p7); // mosi, miso, sclk
LORDTEK 0:b4a14139fab8 13 DigitalOut cs(p8); // chip sellect
LORDTEK 0:b4a14139fab8 14 DigitalOut nextStep(p9); // Step move
LORDTEK 0:b4a14139fab8 15 DigitalOut direction(p10); // Direction
LORDTEK 0:b4a14139fab8 16 Serial pc(USBTX, USBRX); // Used for printf to PC over USB
LORDTEK 0:b4a14139fab8 17 // Delays are in MicroSeconds
LORDTEK 0:b4a14139fab8 18 float stepDelay = 20.0f;
LORDTEK 0:b4a14139fab8 19 float directionDelay = 100.0f;
LORDTEK 0:b4a14139fab8 20 float registerDelay = 5.0f;
LORDTEK 0:b4a14139fab8 21 float tempRead = 0.0f;
LORDTEK 0:b4a14139fab8 22 float tempCompute = 0.0f;
LORDTEK 0:b4a14139fab8 23 uint8_t tempByte = 0;
LORDTEK 0:b4a14139fab8 24 uint16_t readPos = 0;
LORDTEK 0:b4a14139fab8 25
LORDTEK 0:b4a14139fab8 26 void writeReg(uint8_t address, uint8_t value) {
LORDTEK 0:b4a14139fab8 27 cs = 0;
LORDTEK 0:b4a14139fab8 28 amis30543.write(0x80 | (address & 0b11111)); // I do not know why :S, Josh explained why anyway
LORDTEK 0:b4a14139fab8 29 amis30543.write(value);
LORDTEK 0:b4a14139fab8 30 cs = 1;
LORDTEK 0:b4a14139fab8 31 wait_us(registerDelay);
LORDTEK 0:b4a14139fab8 32 }
LORDTEK 0:b4a14139fab8 33
LORDTEK 0:b4a14139fab8 34 void step(void) {
LORDTEK 0:b4a14139fab8 35 nextStep = 1;
LORDTEK 0:b4a14139fab8 36 wait_us(stepDelay);
LORDTEK 0:b4a14139fab8 37 nextStep = 0;
LORDTEK 0:b4a14139fab8 38 wait_us(stepDelay);
LORDTEK 0:b4a14139fab8 39 }
LORDTEK 0:b4a14139fab8 40
LORDTEK 0:b4a14139fab8 41 void changeDir(int dir) {
LORDTEK 0:b4a14139fab8 42 wait_us(directionDelay);
LORDTEK 0:b4a14139fab8 43 if(dir == 0) {
LORDTEK 0:b4a14139fab8 44 writeReg(0x02, 0x80); // CR1 - Change Direction
LORDTEK 0:b4a14139fab8 45 }
LORDTEK 0:b4a14139fab8 46 else {
LORDTEK 0:b4a14139fab8 47 writeReg(0x02, 0x00); // CR1 - Change Direction
LORDTEK 0:b4a14139fab8 48 }
LORDTEK 0:b4a14139fab8 49 wait_us(directionDelay);
LORDTEK 0:b4a14139fab8 50 }
LORDTEK 0:b4a14139fab8 51
LORDTEK 0:b4a14139fab8 52 uint8_t readReg(uint8_t address) {
LORDTEK 0:b4a14139fab8 53 cs = 0;
LORDTEK 0:b4a14139fab8 54 tempByte = amis30543.write(address & 0b11111);
LORDTEK 0:b4a14139fab8 55 cs = 1;
LORDTEK 0:b4a14139fab8 56 return tempByte;
LORDTEK 0:b4a14139fab8 57 }
LORDTEK 0:b4a14139fab8 58
LORDTEK 0:b4a14139fab8 59 /*! Reads a status register and returns the lower 7 bits (the parity bit is
LORDTEK 0:b4a14139fab8 60 * set to 0 in the return value). */
LORDTEK 0:b4a14139fab8 61 uint8_t readStatusReg(uint8_t address) {
LORDTEK 0:b4a14139fab8 62 // Mask off the parity bit.
LORDTEK 0:b4a14139fab8 63 // (Later we might add code here to check the parity
LORDTEK 0:b4a14139fab8 64 // bit and record errors.)
LORDTEK 0:b4a14139fab8 65 return readReg(address) & 0x7F;
LORDTEK 0:b4a14139fab8 66 }
LORDTEK 0:b4a14139fab8 67
LORDTEK 0:b4a14139fab8 68 uint16_t readPosition() {
LORDTEK 0:b4a14139fab8 69 uint8_t sr3 = readStatusReg(0x07); // SR3 0x07
LORDTEK 0:b4a14139fab8 70 uint8_t sr4 = readStatusReg(0x0A); // SR4 0x0A
LORDTEK 0:b4a14139fab8 71 return ((uint16_t)sr3 << 2) | (sr4 & 3);
LORDTEK 0:b4a14139fab8 72 }
LORDTEK 0:b4a14139fab8 73
LORDTEK 0:b4a14139fab8 74 float readPitch() {
LORDTEK 0:b4a14139fab8 75 int i = 0;
LORDTEK 0:b4a14139fab8 76 uint8_t value[13] = {0};
LORDTEK 0:b4a14139fab8 77 uint32_t pyr[3] = {0};
LORDTEK 0:b4a14139fab8 78 float yaw = 0.0f, pitch = 0.0f, roll = 0.0f, tempCheck = 0.0f;
LORDTEK 0:b4a14139fab8 79 // Select the device by seting chip select low
LORDTEK 0:b4a14139fab8 80 yei_cs = 0;
LORDTEK 0:b4a14139fab8 81
LORDTEK 0:b4a14139fab8 82 // Send 0xF6, to start communication on SPI
LORDTEK 0:b4a14139fab8 83 value[0] = yei.write(0xF6);
LORDTEK 0:b4a14139fab8 84 //printf("F6 is sent and 00 is waiting. The return result is: 0x%X\n", value[0]);
LORDTEK 0:b4a14139fab8 85
LORDTEK 0:b4a14139fab8 86 // Send 0x07, to request untared euler angles
LORDTEK 0:b4a14139fab8 87 value[0] = yei.write(0x07);
LORDTEK 0:b4a14139fab8 88 //printf("07 is sent and 04 is waiting. The return result is: 0x%X\n", value[0]);
LORDTEK 0:b4a14139fab8 89
LORDTEK 0:b4a14139fab8 90 for(i=0; i<=12; i++) {
LORDTEK 0:b4a14139fab8 91 value[i] = yei.write(0xFF);
LORDTEK 0:b4a14139fab8 92 if(value[i] == 1) {
LORDTEK 0:b4a14139fab8 93 i = 0;
LORDTEK 0:b4a14139fab8 94 }
LORDTEK 0:b4a14139fab8 95 wait(0.01);
LORDTEK 0:b4a14139fab8 96 }
LORDTEK 0:b4a14139fab8 97
LORDTEK 0:b4a14139fab8 98 pyr[0] = value[1];
LORDTEK 0:b4a14139fab8 99 pyr[0] = (pyr[0] << 8) + value[2];
LORDTEK 0:b4a14139fab8 100 pyr[0] = (pyr[0] << 8) + value[3];
LORDTEK 0:b4a14139fab8 101 pyr[0] = (pyr[0] << 8) + value[4];
LORDTEK 0:b4a14139fab8 102 tempCheck = sp_fp.decoder(pyr[0]) * 180.0f / 3.1415926535897932384626433832795;
LORDTEK 0:b4a14139fab8 103 if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
LORDTEK 0:b4a14139fab8 104 pitch = tempCheck;
LORDTEK 0:b4a14139fab8 105 pyr[1] = value[5];
LORDTEK 0:b4a14139fab8 106 pyr[1] = (pyr[1] << 8) + value[6];
LORDTEK 0:b4a14139fab8 107 pyr[1] = (pyr[1] << 8) + value[7];
LORDTEK 0:b4a14139fab8 108 pyr[1] = (pyr[1] << 8) + value[8];
LORDTEK 0:b4a14139fab8 109 tempCheck = sp_fp.decoder(pyr[1]) * 180.0f / 3.1415926535897932384626433832795;
LORDTEK 0:b4a14139fab8 110 if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
LORDTEK 0:b4a14139fab8 111 yaw = tempCheck;
LORDTEK 0:b4a14139fab8 112 pyr[2] = value[9];
LORDTEK 0:b4a14139fab8 113 pyr[2] = (pyr[2] << 8) + value[10];
LORDTEK 0:b4a14139fab8 114 pyr[2] = (pyr[2] << 8) + value[11];
LORDTEK 0:b4a14139fab8 115 pyr[2] = (pyr[2] << 8) + value[12];
LORDTEK 0:b4a14139fab8 116 tempCheck = sp_fp.decoder(pyr[2]) * 180.0f / 3.1415926535897932384626433832795;
LORDTEK 0:b4a14139fab8 117 if(tempCheck >= -180.0f && tempCheck <= 180.0f && tempCheck != 0.0f && tempCheck != -0.0f)
LORDTEK 0:b4a14139fab8 118 roll = tempCheck;
LORDTEK 0:b4a14139fab8 119
LORDTEK 0:b4a14139fab8 120 pc.printf("Pitch: %f Yaw: %f Roll: %f\n", pitch, yaw, roll);
LORDTEK 0:b4a14139fab8 121
LORDTEK 0:b4a14139fab8 122 // Deselect the device
LORDTEK 0:b4a14139fab8 123 yei_cs = 1;
LORDTEK 0:b4a14139fab8 124 return pitch;
LORDTEK 0:b4a14139fab8 125 }
LORDTEK 0:b4a14139fab8 126
LORDTEK 0:b4a14139fab8 127 int main() {
LORDTEK 0:b4a14139fab8 128 yei_cs = 1;
LORDTEK 0:b4a14139fab8 129 yei.format(8,0);
LORDTEK 0:b4a14139fab8 130 yei.frequency(1000000);
LORDTEK 0:b4a14139fab8 131 pc.baud(9600);
LORDTEK 0:b4a14139fab8 132 controller.setInputLimits(-10.0, 10.0);
LORDTEK 0:b4a14139fab8 133 controller.setOutputLimits(-1000, 1000);
LORDTEK 0:b4a14139fab8 134 controller.setBias(0.0001);
LORDTEK 0:b4a14139fab8 135 controller.setMode(AUTO);
LORDTEK 0:b4a14139fab8 136 controller.setSetPoint(-3.2);
LORDTEK 0:b4a14139fab8 137 int i = 0;
LORDTEK 0:b4a14139fab8 138 wait(1);
LORDTEK 0:b4a14139fab8 139
LORDTEK 0:b4a14139fab8 140 writeReg(0x03, 0x80); // CR2
LORDTEK 0:b4a14139fab8 141 writeReg(0x00, 0x00); // WR
LORDTEK 0:b4a14139fab8 142 // for CR0: 132mA->X0, 485mA->X5, 955mA-> XC
LORDTEK 0:b4a14139fab8 143 writeReg(0x01, 0x00); // CR0
LORDTEK 0:b4a14139fab8 144 writeReg(0x02, 0x80); // CR1
LORDTEK 0:b4a14139fab8 145 writeReg(0x09, 0x01); // CR3
LORDTEK 0:b4a14139fab8 146
LORDTEK 0:b4a14139fab8 147 wait_us(registerDelay);
LORDTEK 0:b4a14139fab8 148 while(1) {
LORDTEK 0:b4a14139fab8 149 tempRead = readPitch();tempRead+=4.0f;
LORDTEK 0:b4a14139fab8 150 //controller.setProcessValue(tempRead);
LORDTEK 0:b4a14139fab8 151 //tempCompute = controller.compute();
LORDTEK 0:b4a14139fab8 152 //pc.printf("PID:%f \t pitch:%f\n", tempCompute, tempRead);
LORDTEK 0:b4a14139fab8 153 wait(RATE);
LORDTEK 0:b4a14139fab8 154 if(tempRead < 0) {
LORDTEK 0:b4a14139fab8 155 changeDir(0);
LORDTEK 0:b4a14139fab8 156 step();
LORDTEK 0:b4a14139fab8 157 /*for(i=0; i>=tempCompute; i--) {
LORDTEK 0:b4a14139fab8 158 step();
LORDTEK 0:b4a14139fab8 159 }*/
LORDTEK 0:b4a14139fab8 160 }
LORDTEK 0:b4a14139fab8 161 else {
LORDTEK 0:b4a14139fab8 162 changeDir(1);
LORDTEK 0:b4a14139fab8 163 step();
LORDTEK 0:b4a14139fab8 164 /*for(i=0; i<=tempCompute; i++) {
LORDTEK 0:b4a14139fab8 165 step();
LORDTEK 0:b4a14139fab8 166 }*/
LORDTEK 0:b4a14139fab8 167 }
LORDTEK 0:b4a14139fab8 168 }
LORDTEK 0:b4a14139fab8 169 }