Seunghee Jeong
/
controller_real
,
main.cpp@0:9d06cdd73bc7, 22 months ago (annotated)
- Committer:
- jjeong
- Date:
- Sat Dec 03 08:21:37 2022 +0000
- Revision:
- 0:9d06cdd73bc7
controller for quadrotor with RF24 driver;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jjeong | 0:9d06cdd73bc7 | 1 | #include "mbed.h" |
jjeong | 0:9d06cdd73bc7 | 2 | #include "nRF24L01.h" |
jjeong | 0:9d06cdd73bc7 | 3 | #include "RF24.h" |
jjeong | 0:9d06cdd73bc7 | 4 | #include "RF24_config.h" |
jjeong | 0:9d06cdd73bc7 | 5 | |
jjeong | 0:9d06cdd73bc7 | 6 | Serial pc(PB_6, PB_7); //D5, D4 //PB_6, PB_7 |
jjeong | 0:9d06cdd73bc7 | 7 | I2C i2c(PA_10, PA_9); //sda, scl PA_10, PA_9 D0, D1 |
jjeong | 0:9d06cdd73bc7 | 8 | //(PinName mosi, PinName miso, PinName sck, PinName _cepin, PinName _csnpin) |
jjeong | 0:9d06cdd73bc7 | 9 | RF24 NRF24L01(PB_5, PB_4, PB_3, PA_12, PA_11); |
jjeong | 0:9d06cdd73bc7 | 10 | //RF24 NRF24L01(D11, D12, D13, D2, D10); |
jjeong | 0:9d06cdd73bc7 | 11 | |
jjeong | 0:9d06cdd73bc7 | 12 | AnalogIn JOYPIN1(PA_0); //A0 |
jjeong | 0:9d06cdd73bc7 | 13 | AnalogIn JOYPIN2(PA_1); //A1 |
jjeong | 0:9d06cdd73bc7 | 14 | AnalogIn JOYPIN3(PA_3); //A2 |
jjeong | 0:9d06cdd73bc7 | 15 | AnalogIn JOYPIN4(PA_4); //A3 |
jjeong | 0:9d06cdd73bc7 | 16 | |
jjeong | 0:9d06cdd73bc7 | 17 | AnalogIn POT1(PA_7); //A6 |
jjeong | 0:9d06cdd73bc7 | 18 | AnalogIn POT2(PA_6); //A5 |
jjeong | 0:9d06cdd73bc7 | 19 | AnalogIn POT3(PA_5); //A4 |
jjeong | 0:9d06cdd73bc7 | 20 | |
jjeong | 0:9d06cdd73bc7 | 21 | DigitalIn BUTTON1(PA_2); //A7 |
jjeong | 0:9d06cdd73bc7 | 22 | DigitalIn BUTTON2(PA_8); //D9 |
jjeong | 0:9d06cdd73bc7 | 23 | |
jjeong | 0:9d06cdd73bc7 | 24 | DigitalOut MCU_LED(PB_0); //D3 |
jjeong | 0:9d06cdd73bc7 | 25 | DigitalOut MCU_LED2(PB_1); //D6 |
jjeong | 0:9d06cdd73bc7 | 26 | |
jjeong | 0:9d06cdd73bc7 | 27 | //MPU |
jjeong | 0:9d06cdd73bc7 | 28 | float mpu_temp_read; |
jjeong | 0:9d06cdd73bc7 | 29 | int32_t t_fine; |
jjeong | 0:9d06cdd73bc7 | 30 | int16_t gyro_raw[3], acc_raw[3], mag_raw[3]; |
jjeong | 0:9d06cdd73bc7 | 31 | float gyro_offset[3], acc_offset[3]; |
jjeong | 0:9d06cdd73bc7 | 32 | float mag_factory[3]; |
jjeong | 0:9d06cdd73bc7 | 33 | int16_t angle[2]; |
jjeong | 0:9d06cdd73bc7 | 34 | |
jjeong | 0:9d06cdd73bc7 | 35 | //magnetometer |
jjeong | 0:9d06cdd73bc7 | 36 | float x_offset = -1320.5, y_offset = 0, z_offset = -633.25; |
jjeong | 0:9d06cdd73bc7 | 37 | //float x_offset = -1320.5, y_offset = 1301.8, z_offset = -633.25; |
jjeong | 0:9d06cdd73bc7 | 38 | float x_scale = 0.9819, y_scale = 0.9957, z_scale = 1.0233; |
jjeong | 0:9d06cdd73bc7 | 39 | |
jjeong | 0:9d06cdd73bc7 | 40 | //RF |
jjeong | 0:9d06cdd73bc7 | 41 | const uint64_t pipe = 0x1212121212LL; |
jjeong | 0:9d06cdd73bc7 | 42 | float PITCH2 = 0, ROLL2 = 0, YAW2 = 0, THROTTLE2 = 0; |
jjeong | 0:9d06cdd73bc7 | 43 | int16_t PITCH = 0, ROLL = 0, YAW = 0, THROTTLE = 0; |
jjeong | 0:9d06cdd73bc7 | 44 | int8_t recv[30]; |
jjeong | 0:9d06cdd73bc7 | 45 | int8_t trans[30]; |
jjeong | 0:9d06cdd73bc7 | 46 | int8_t ackMessage[30]; |
jjeong | 0:9d06cdd73bc7 | 47 | int16_t ackMessage2[30]; |
jjeong | 0:9d06cdd73bc7 | 48 | uint32_t ackMessage3; |
jjeong | 0:9d06cdd73bc7 | 49 | int32_t ackMessage4[4]; |
jjeong | 0:9d06cdd73bc7 | 50 | int8_t ackMessage5[3]; |
jjeong | 0:9d06cdd73bc7 | 51 | |
jjeong | 0:9d06cdd73bc7 | 52 | //Timer Loop |
jjeong | 0:9d06cdd73bc7 | 53 | Timer timer1; |
jjeong | 0:9d06cdd73bc7 | 54 | |
jjeong | 0:9d06cdd73bc7 | 55 | int8_t TEMP_BUTTON1, TEMP_BUTTON2; |
jjeong | 0:9d06cdd73bc7 | 56 | int8_t temp_led = 1, temp_led2 = -1; |
jjeong | 0:9d06cdd73bc7 | 57 | int loop_count = 0, loop_count2 = 0, loop_count3 = 0, loop_count4 = 0, loop_count5 = 0; |
jjeong | 0:9d06cdd73bc7 | 58 | int loop_count4_max = 50; |
jjeong | 0:9d06cdd73bc7 | 59 | float OFFSET1, OFFSET2, OFFSET3; |
jjeong | 0:9d06cdd73bc7 | 60 | |
jjeong | 0:9d06cdd73bc7 | 61 | int8_t current_mode = 0; |
jjeong | 0:9d06cdd73bc7 | 62 | |
jjeong | 0:9d06cdd73bc7 | 63 | int8_t toggle_mode = 1; |
jjeong | 0:9d06cdd73bc7 | 64 | |
jjeong | 0:9d06cdd73bc7 | 65 | //rf ack |
jjeong | 0:9d06cdd73bc7 | 66 | int8_t ack_count = 0; |
jjeong | 0:9d06cdd73bc7 | 67 | uint8_t cnt = 0; |
jjeong | 0:9d06cdd73bc7 | 68 | |
jjeong | 0:9d06cdd73bc7 | 69 | int32_t temp_time; |
jjeong | 0:9d06cdd73bc7 | 70 | uint8_t temp_time2; |
jjeong | 0:9d06cdd73bc7 | 71 | |
jjeong | 0:9d06cdd73bc7 | 72 | //----------------------------------------------------------------------------------------- |
jjeong | 0:9d06cdd73bc7 | 73 | |
jjeong | 0:9d06cdd73bc7 | 74 | int16_t constrain_int16(int16_t x, int16_t min, int16_t max) |
jjeong | 0:9d06cdd73bc7 | 75 | { |
jjeong | 0:9d06cdd73bc7 | 76 | if (x > max) x = max; |
jjeong | 0:9d06cdd73bc7 | 77 | else if (x < min) x = min; |
jjeong | 0:9d06cdd73bc7 | 78 | |
jjeong | 0:9d06cdd73bc7 | 79 | return x; |
jjeong | 0:9d06cdd73bc7 | 80 | } |
jjeong | 0:9d06cdd73bc7 | 81 | |
jjeong | 0:9d06cdd73bc7 | 82 | int32_t constrain_int32(int32_t x, int32_t min, int32_t max) |
jjeong | 0:9d06cdd73bc7 | 83 | { |
jjeong | 0:9d06cdd73bc7 | 84 | if (x > max) x = max; |
jjeong | 0:9d06cdd73bc7 | 85 | else if (x < min) x = min; |
jjeong | 0:9d06cdd73bc7 | 86 | |
jjeong | 0:9d06cdd73bc7 | 87 | return x; |
jjeong | 0:9d06cdd73bc7 | 88 | } |
jjeong | 0:9d06cdd73bc7 | 89 | |
jjeong | 0:9d06cdd73bc7 | 90 | float constrain_float(float x, float min, float max) |
jjeong | 0:9d06cdd73bc7 | 91 | { |
jjeong | 0:9d06cdd73bc7 | 92 | if (x > max) x = max; |
jjeong | 0:9d06cdd73bc7 | 93 | else if (x < min) x = min; |
jjeong | 0:9d06cdd73bc7 | 94 | |
jjeong | 0:9d06cdd73bc7 | 95 | return x; |
jjeong | 0:9d06cdd73bc7 | 96 | } |
jjeong | 0:9d06cdd73bc7 | 97 | |
jjeong | 0:9d06cdd73bc7 | 98 | //------------------------------------------------------------------------------------------------ |
jjeong | 0:9d06cdd73bc7 | 99 | int main() { |
jjeong | 0:9d06cdd73bc7 | 100 | |
jjeong | 0:9d06cdd73bc7 | 101 | //disableDebugPorts(); |
jjeong | 0:9d06cdd73bc7 | 102 | //pc.baud(230400); //115200 |
jjeong | 0:9d06cdd73bc7 | 103 | //i2c.frequency(400000); |
jjeong | 0:9d06cdd73bc7 | 104 | |
jjeong | 0:9d06cdd73bc7 | 105 | //RF |
jjeong | 0:9d06cdd73bc7 | 106 | NRF24L01.begin(); |
jjeong | 0:9d06cdd73bc7 | 107 | NRF24L01.setDataRate(RF24_2MBPS); //RF24_2MBPS |
jjeong | 0:9d06cdd73bc7 | 108 | NRF24L01.setChannel(22); |
jjeong | 0:9d06cdd73bc7 | 109 | NRF24L01.setPayloadSize(28); //28 |
jjeong | 0:9d06cdd73bc7 | 110 | NRF24L01.setAddressWidth(5); |
jjeong | 0:9d06cdd73bc7 | 111 | NRF24L01.setRetries(2,4); //1,3 2,8 |
jjeong | 0:9d06cdd73bc7 | 112 | |
jjeong | 0:9d06cdd73bc7 | 113 | NRF24L01.enableAckPayload(); |
jjeong | 0:9d06cdd73bc7 | 114 | NRF24L01.openWritingPipe(pipe); |
jjeong | 0:9d06cdd73bc7 | 115 | pc.printf("start\n\r"); |
jjeong | 0:9d06cdd73bc7 | 116 | int counta=0; |
jjeong | 0:9d06cdd73bc7 | 117 | timer1.start(); |
jjeong | 0:9d06cdd73bc7 | 118 | |
jjeong | 0:9d06cdd73bc7 | 119 | //------------------------------------------------------------------------------------------------ |
jjeong | 0:9d06cdd73bc7 | 120 | while(1) { |
jjeong | 0:9d06cdd73bc7 | 121 | |
jjeong | 0:9d06cdd73bc7 | 122 | loop_count = loop_count + 1; |
jjeong | 0:9d06cdd73bc7 | 123 | loop_count2 = loop_count2 + 1; |
jjeong | 0:9d06cdd73bc7 | 124 | loop_count4 = loop_count4 + 1; |
jjeong | 0:9d06cdd73bc7 | 125 | |
jjeong | 0:9d06cdd73bc7 | 126 | TEMP_BUTTON1 = (int8_t)BUTTON1; |
jjeong | 0:9d06cdd73bc7 | 127 | TEMP_BUTTON2 = (int8_t)BUTTON2; |
jjeong | 0:9d06cdd73bc7 | 128 | |
jjeong | 0:9d06cdd73bc7 | 129 | THROTTLE2 = JOYPIN1.read(); //left side vertical |
jjeong | 0:9d06cdd73bc7 | 130 | PITCH2 = JOYPIN3.read() - 0.5f; //65536 //up positive |
jjeong | 0:9d06cdd73bc7 | 131 | ROLL2 = JOYPIN4.read() - 0.5f; //right positive |
jjeong | 0:9d06cdd73bc7 | 132 | YAW2 = JOYPIN2.read() - 0.5f; // right positive |
jjeong | 0:9d06cdd73bc7 | 133 | |
jjeong | 0:9d06cdd73bc7 | 134 | OFFSET1 = POT1.read() - 0.5f; //left //reduce value counter-clockwise |
jjeong | 0:9d06cdd73bc7 | 135 | OFFSET2 = POT2.read() - 0.5f; //middle |
jjeong | 0:9d06cdd73bc7 | 136 | OFFSET3 = POT3.read() - 0.5f; //right |
jjeong | 0:9d06cdd73bc7 | 137 | //pc.printf("%f, %f, %f\n\r", OFFSET1, OFFSET2, OFFSET3); |
jjeong | 0:9d06cdd73bc7 | 138 | |
jjeong | 0:9d06cdd73bc7 | 139 | THROTTLE = (int16_t)(THROTTLE2 * 1024 * 1.15f); |
jjeong | 0:9d06cdd73bc7 | 140 | THROTTLE = constrain_int16(THROTTLE, 0, 1500); |
jjeong | 0:9d06cdd73bc7 | 141 | |
jjeong | 0:9d06cdd73bc7 | 142 | PITCH = -(int16_t)((PITCH2 * 1024 / 2 - OFFSET2 * 1024 / 4)); //- |
jjeong | 0:9d06cdd73bc7 | 143 | ROLL = -(int16_t)((ROLL2 * 1024 / 2 - OFFSET1 * 1024 / 4)); //- |
jjeong | 0:9d06cdd73bc7 | 144 | YAW = (int16_t)((- YAW2 * 1024 / 6 + OFFSET3 * 1024 / 2)); |
jjeong | 0:9d06cdd73bc7 | 145 | |
jjeong | 0:9d06cdd73bc7 | 146 | if (loop_count >= 4){ |
jjeong | 0:9d06cdd73bc7 | 147 | |
jjeong | 0:9d06cdd73bc7 | 148 | trans[0] = ((char *)&ROLL)[0]; |
jjeong | 0:9d06cdd73bc7 | 149 | trans[1] = ((char *)&ROLL)[1]; |
jjeong | 0:9d06cdd73bc7 | 150 | trans[2] = ((char *)&PITCH)[0]; //0;// |
jjeong | 0:9d06cdd73bc7 | 151 | trans[3] = ((char *)&PITCH)[1]; |
jjeong | 0:9d06cdd73bc7 | 152 | trans[4] = ((char *)&YAW)[0]; |
jjeong | 0:9d06cdd73bc7 | 153 | trans[5] = ((char *)&YAW)[1]; |
jjeong | 0:9d06cdd73bc7 | 154 | trans[6] = ((char *)&THROTTLE)[0]; |
jjeong | 0:9d06cdd73bc7 | 155 | trans[7] = ((char *)&THROTTLE)[1]; |
jjeong | 0:9d06cdd73bc7 | 156 | trans[8] = (char)BUTTON1; |
jjeong | 0:9d06cdd73bc7 | 157 | trans[9] = (char)BUTTON2; |
jjeong | 0:9d06cdd73bc7 | 158 | printf("transmit\t%d",counta); |
jjeong | 0:9d06cdd73bc7 | 159 | counta++; |
jjeong | 0:9d06cdd73bc7 | 160 | NRF24L01.powerUp(); |
jjeong | 0:9d06cdd73bc7 | 161 | NRF24L01.write(trans, 10); |
jjeong | 0:9d06cdd73bc7 | 162 | |
jjeong | 0:9d06cdd73bc7 | 163 | /* |
jjeong | 0:9d06cdd73bc7 | 164 | if (NRF24L01.isAckPayloadAvailable()){ |
jjeong | 0:9d06cdd73bc7 | 165 | NRF24L01.read(ackMessage, 14); //28 23 |
jjeong | 0:9d06cdd73bc7 | 166 | |
jjeong | 0:9d06cdd73bc7 | 167 | ackMessage2[0] = *(int16_t*)(&ackMessage[0]); |
jjeong | 0:9d06cdd73bc7 | 168 | ackMessage2[1] = *(int16_t*)(&ackMessage[2]); |
jjeong | 0:9d06cdd73bc7 | 169 | ackMessage2[2] = *(int16_t*)(&ackMessage[4]); |
jjeong | 0:9d06cdd73bc7 | 170 | |
jjeong | 0:9d06cdd73bc7 | 171 | ackMessage2[3] = *(int16_t*)(&ackMessage[6]); |
jjeong | 0:9d06cdd73bc7 | 172 | ackMessage2[4] = *(int16_t*)(&ackMessage[8]); |
jjeong | 0:9d06cdd73bc7 | 173 | |
jjeong | 0:9d06cdd73bc7 | 174 | ackMessage2[5] = *(int16_t*)(&ackMessage[10]); |
jjeong | 0:9d06cdd73bc7 | 175 | ackMessage2[6] = *(int16_t*)(&ackMessage[12]); |
jjeong | 0:9d06cdd73bc7 | 176 | |
jjeong | 0:9d06cdd73bc7 | 177 | temp_time2 = (uint8_t)(temp_time / 100); //1000 |
jjeong | 0:9d06cdd73bc7 | 178 | |
jjeong | 0:9d06cdd73bc7 | 179 | temp_time = 0; |
jjeong | 0:9d06cdd73bc7 | 180 | |
jjeong | 0:9d06cdd73bc7 | 181 | } |
jjeong | 0:9d06cdd73bc7 | 182 | */ |
jjeong | 0:9d06cdd73bc7 | 183 | |
jjeong | 0:9d06cdd73bc7 | 184 | loop_count = 0; |
jjeong | 0:9d06cdd73bc7 | 185 | } |
jjeong | 0:9d06cdd73bc7 | 186 | |
jjeong | 0:9d06cdd73bc7 | 187 | if (loop_count4 >= loop_count4_max){ |
jjeong | 0:9d06cdd73bc7 | 188 | |
jjeong | 0:9d06cdd73bc7 | 189 | if (temp_led == 1) { |
jjeong | 0:9d06cdd73bc7 | 190 | MCU_LED = 1; |
jjeong | 0:9d06cdd73bc7 | 191 | //MCU_LED2 = 0; |
jjeong | 0:9d06cdd73bc7 | 192 | } |
jjeong | 0:9d06cdd73bc7 | 193 | if (temp_led == -1) { |
jjeong | 0:9d06cdd73bc7 | 194 | MCU_LED = 0; |
jjeong | 0:9d06cdd73bc7 | 195 | //MCU_LED2 = 1; |
jjeong | 0:9d06cdd73bc7 | 196 | } |
jjeong | 0:9d06cdd73bc7 | 197 | temp_led = -1 * temp_led; |
jjeong | 0:9d06cdd73bc7 | 198 | loop_count4 = 0; |
jjeong | 0:9d06cdd73bc7 | 199 | } |
jjeong | 0:9d06cdd73bc7 | 200 | |
jjeong | 0:9d06cdd73bc7 | 201 | while (timer1.read_us() < 2500); //400 Hz |
jjeong | 0:9d06cdd73bc7 | 202 | |
jjeong | 0:9d06cdd73bc7 | 203 | temp_time = temp_time + timer1.read_us(); |
jjeong | 0:9d06cdd73bc7 | 204 | |
jjeong | 0:9d06cdd73bc7 | 205 | timer1.reset(); |
jjeong | 0:9d06cdd73bc7 | 206 | |
jjeong | 0:9d06cdd73bc7 | 207 | } |
jjeong | 0:9d06cdd73bc7 | 208 | |
jjeong | 0:9d06cdd73bc7 | 209 | } |
jjeong | 0:9d06cdd73bc7 | 210 |