LORDTEK LORDTEK
/
AMIS30543
AMIS 30543 Basic
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?
User | Revision | Line number | New 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 | } |