BME SmartLab
/
SmartCone
SmartCone Demo
main.cpp@0:f86c91eb17cb, 2017-01-19 (annotated)
- Committer:
- mrcrsch
- Date:
- Thu Jan 19 13:25:53 2017 +0000
- Revision:
- 0:f86c91eb17cb
Init for publish;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mrcrsch | 0:f86c91eb17cb | 1 | #include "mbed.h" |
mrcrsch | 0:f86c91eb17cb | 2 | #include "MPU9250Mod.h" |
mrcrsch | 0:f86c91eb17cb | 3 | #include "RF24.h" |
mrcrsch | 0:f86c91eb17cb | 4 | |
mrcrsch | 0:f86c91eb17cb | 5 | |
mrcrsch | 0:f86c91eb17cb | 6 | #define NodeID 1 |
mrcrsch | 0:f86c91eb17cb | 7 | |
mrcrsch | 0:f86c91eb17cb | 8 | |
mrcrsch | 0:f86c91eb17cb | 9 | #define ledpin PA_10 |
mrcrsch | 0:f86c91eb17cb | 10 | |
mrcrsch | 0:f86c91eb17cb | 11 | #define nrf_CE PA_1 |
mrcrsch | 0:f86c91eb17cb | 12 | #define nrf_CSN PA_4 |
mrcrsch | 0:f86c91eb17cb | 13 | #define spi_SCK PA_5 |
mrcrsch | 0:f86c91eb17cb | 14 | #define spi_MOSI PA_7 |
mrcrsch | 0:f86c91eb17cb | 15 | #define spi_MISO PA_6 |
mrcrsch | 0:f86c91eb17cb | 16 | #define nrf_irq PA_1 |
mrcrsch | 0:f86c91eb17cb | 17 | |
mrcrsch | 0:f86c91eb17cb | 18 | #define i2c_sda PF_0 |
mrcrsch | 0:f86c91eb17cb | 19 | #define i2c_scl PF_1 |
mrcrsch | 0:f86c91eb17cb | 20 | |
mrcrsch | 0:f86c91eb17cb | 21 | MPU9250 mpu9250; |
mrcrsch | 0:f86c91eb17cb | 22 | DigitalOut led(ledpin); |
mrcrsch | 0:f86c91eb17cb | 23 | |
mrcrsch | 0:f86c91eb17cb | 24 | RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN ); |
mrcrsch | 0:f86c91eb17cb | 25 | InterruptIn NRF_irq(PA_0); |
mrcrsch | 0:f86c91eb17cb | 26 | |
mrcrsch | 0:f86c91eb17cb | 27 | const uint64_t PiAddress = 0xF0F0F0F0E1LL; |
mrcrsch | 0:f86c91eb17cb | 28 | const uint64_t ActionAddress = 0xF0F0F0F0D2LL; |
mrcrsch | 0:f86c91eb17cb | 29 | |
mrcrsch | 0:f86c91eb17cb | 30 | |
mrcrsch | 0:f86c91eb17cb | 31 | struct IRQStruct{ |
mrcrsch | 0:f86c91eb17cb | 32 | uint8_t ID; |
mrcrsch | 0:f86c91eb17cb | 33 | uint8_t Threshold; |
mrcrsch | 0:f86c91eb17cb | 34 | bool isOn; |
mrcrsch | 0:f86c91eb17cb | 35 | }IRQMsg; |
mrcrsch | 0:f86c91eb17cb | 36 | |
mrcrsch | 0:f86c91eb17cb | 37 | void RF24IntHandler(); |
mrcrsch | 0:f86c91eb17cb | 38 | void SetupRadio(); |
mrcrsch | 0:f86c91eb17cb | 39 | void SendMsg(); |
mrcrsch | 0:f86c91eb17cb | 40 | |
mrcrsch | 0:f86c91eb17cb | 41 | float AccelMsg = 1.0; |
mrcrsch | 0:f86c91eb17cb | 42 | uint8_t AThreshold = 5; |
mrcrsch | 0:f86c91eb17cb | 43 | |
mrcrsch | 0:f86c91eb17cb | 44 | typedef enum{ |
mrcrsch | 0:f86c91eb17cb | 45 | NRF, |
mrcrsch | 0:f86c91eb17cb | 46 | IMU |
mrcrsch | 0:f86c91eb17cb | 47 | } state; |
mrcrsch | 0:f86c91eb17cb | 48 | |
mrcrsch | 0:f86c91eb17cb | 49 | state State = IMU; |
mrcrsch | 0:f86c91eb17cb | 50 | |
mrcrsch | 0:f86c91eb17cb | 51 | |
mrcrsch | 0:f86c91eb17cb | 52 | int main(){ |
mrcrsch | 0:f86c91eb17cb | 53 | led = !led; |
mrcrsch | 0:f86c91eb17cb | 54 | SetupRadio(); |
mrcrsch | 0:f86c91eb17cb | 55 | |
mrcrsch | 0:f86c91eb17cb | 56 | NRF_irq.fall(&RF24IntHandler); |
mrcrsch | 0:f86c91eb17cb | 57 | |
mrcrsch | 0:f86c91eb17cb | 58 | |
mrcrsch | 0:f86c91eb17cb | 59 | |
mrcrsch | 0:f86c91eb17cb | 60 | i2c.frequency(400000); |
mrcrsch | 0:f86c91eb17cb | 61 | mpu9250.initMPU9250(); |
mrcrsch | 0:f86c91eb17cb | 62 | |
mrcrsch | 0:f86c91eb17cb | 63 | |
mrcrsch | 0:f86c91eb17cb | 64 | mpu9250.getAres(); // Get accelerometer sensitivity |
mrcrsch | 0:f86c91eb17cb | 65 | mpu9250.getGres(); // Get gyro sensitivity |
mrcrsch | 0:f86c91eb17cb | 66 | mpu9250.getMres(); // Get magnetometer sensitivity |
mrcrsch | 0:f86c91eb17cb | 67 | |
mrcrsch | 0:f86c91eb17cb | 68 | while(true){ |
mrcrsch | 0:f86c91eb17cb | 69 | |
mrcrsch | 0:f86c91eb17cb | 70 | switch(State){ |
mrcrsch | 0:f86c91eb17cb | 71 | case(IMU): |
mrcrsch | 0:f86c91eb17cb | 72 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01){ |
mrcrsch | 0:f86c91eb17cb | 73 | mpu9250.readAccelData(accelCount); |
mrcrsch | 0:f86c91eb17cb | 74 | ax = (float)accelCount[0]*aRes; |
mrcrsch | 0:f86c91eb17cb | 75 | ay = (float)accelCount[1]*aRes; |
mrcrsch | 0:f86c91eb17cb | 76 | az = (float)accelCount[2]*aRes; |
mrcrsch | 0:f86c91eb17cb | 77 | |
mrcrsch | 0:f86c91eb17cb | 78 | float AbsAcc = sqrt(ax*ax + ay*ay + az*az); |
mrcrsch | 0:f86c91eb17cb | 79 | AccelMsg = AbsAcc; |
mrcrsch | 0:f86c91eb17cb | 80 | //memcpy(&AccelMsg, &AbsAcc, 4); |
mrcrsch | 0:f86c91eb17cb | 81 | |
mrcrsch | 0:f86c91eb17cb | 82 | if(AbsAcc > AThreshold) |
mrcrsch | 0:f86c91eb17cb | 83 | SendMsg(); |
mrcrsch | 0:f86c91eb17cb | 84 | } |
mrcrsch | 0:f86c91eb17cb | 85 | |
mrcrsch | 0:f86c91eb17cb | 86 | wait(0.1); |
mrcrsch | 0:f86c91eb17cb | 87 | |
mrcrsch | 0:f86c91eb17cb | 88 | |
mrcrsch | 0:f86c91eb17cb | 89 | break; |
mrcrsch | 0:f86c91eb17cb | 90 | |
mrcrsch | 0:f86c91eb17cb | 91 | case(NRF): |
mrcrsch | 0:f86c91eb17cb | 92 | if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed |
mrcrsch | 0:f86c91eb17cb | 93 | continue; |
mrcrsch | 0:f86c91eb17cb | 94 | |
mrcrsch | 0:f86c91eb17cb | 95 | radio.read(&IRQMsg, sizeof(IRQMsg)); |
mrcrsch | 0:f86c91eb17cb | 96 | |
mrcrsch | 0:f86c91eb17cb | 97 | if(IRQMsg.ID != NodeID) |
mrcrsch | 0:f86c91eb17cb | 98 | continue; |
mrcrsch | 0:f86c91eb17cb | 99 | |
mrcrsch | 0:f86c91eb17cb | 100 | led = IRQMsg.isOn; |
mrcrsch | 0:f86c91eb17cb | 101 | AThreshold = IRQMsg.Threshold; |
mrcrsch | 0:f86c91eb17cb | 102 | |
mrcrsch | 0:f86c91eb17cb | 103 | State = IMU; |
mrcrsch | 0:f86c91eb17cb | 104 | |
mrcrsch | 0:f86c91eb17cb | 105 | break; |
mrcrsch | 0:f86c91eb17cb | 106 | } |
mrcrsch | 0:f86c91eb17cb | 107 | |
mrcrsch | 0:f86c91eb17cb | 108 | |
mrcrsch | 0:f86c91eb17cb | 109 | |
mrcrsch | 0:f86c91eb17cb | 110 | |
mrcrsch | 0:f86c91eb17cb | 111 | |
mrcrsch | 0:f86c91eb17cb | 112 | |
mrcrsch | 0:f86c91eb17cb | 113 | |
mrcrsch | 0:f86c91eb17cb | 114 | |
mrcrsch | 0:f86c91eb17cb | 115 | |
mrcrsch | 0:f86c91eb17cb | 116 | } |
mrcrsch | 0:f86c91eb17cb | 117 | } |
mrcrsch | 0:f86c91eb17cb | 118 | |
mrcrsch | 0:f86c91eb17cb | 119 | void SendMsg(){ |
mrcrsch | 0:f86c91eb17cb | 120 | radio.stopListening(); |
mrcrsch | 0:f86c91eb17cb | 121 | wait_ms(10); |
mrcrsch | 0:f86c91eb17cb | 122 | |
mrcrsch | 0:f86c91eb17cb | 123 | |
mrcrsch | 0:f86c91eb17cb | 124 | radio.write(&AccelMsg, sizeof(AccelMsg) ); |
mrcrsch | 0:f86c91eb17cb | 125 | radio.startListening(); |
mrcrsch | 0:f86c91eb17cb | 126 | led = !led; |
mrcrsch | 0:f86c91eb17cb | 127 | wait(1); |
mrcrsch | 0:f86c91eb17cb | 128 | |
mrcrsch | 0:f86c91eb17cb | 129 | } |
mrcrsch | 0:f86c91eb17cb | 130 | |
mrcrsch | 0:f86c91eb17cb | 131 | void SetupRadio(){ |
mrcrsch | 0:f86c91eb17cb | 132 | |
mrcrsch | 0:f86c91eb17cb | 133 | radio.begin(); |
mrcrsch | 0:f86c91eb17cb | 134 | radio.setPALevel(RF24_PA_MAX) ; |
mrcrsch | 0:f86c91eb17cb | 135 | radio.setDataRate(RF24_2MBPS); |
mrcrsch | 0:f86c91eb17cb | 136 | radio.setCRCLength(RF24_CRC_16); |
mrcrsch | 0:f86c91eb17cb | 137 | radio.setChannel(120); |
mrcrsch | 0:f86c91eb17cb | 138 | |
mrcrsch | 0:f86c91eb17cb | 139 | radio.enableDynamicAck(); |
mrcrsch | 0:f86c91eb17cb | 140 | radio.enableDynamicPayloads(); |
mrcrsch | 0:f86c91eb17cb | 141 | |
mrcrsch | 0:f86c91eb17cb | 142 | radio.openWritingPipe(ActionAddress); |
mrcrsch | 0:f86c91eb17cb | 143 | radio.openReadingPipe(1,PiAddress); |
mrcrsch | 0:f86c91eb17cb | 144 | |
mrcrsch | 0:f86c91eb17cb | 145 | radio.startListening(); |
mrcrsch | 0:f86c91eb17cb | 146 | |
mrcrsch | 0:f86c91eb17cb | 147 | } |
mrcrsch | 0:f86c91eb17cb | 148 | |
mrcrsch | 0:f86c91eb17cb | 149 | |
mrcrsch | 0:f86c91eb17cb | 150 | |
mrcrsch | 0:f86c91eb17cb | 151 | void RF24IntHandler(){ |
mrcrsch | 0:f86c91eb17cb | 152 | State = NRF; |
mrcrsch | 0:f86c91eb17cb | 153 | |
mrcrsch | 0:f86c91eb17cb | 154 | |
mrcrsch | 0:f86c91eb17cb | 155 | } |