Seunghee Jeong / Mbed 2 deprecated controller_real

,

Dependencies:   mbed

main.cpp@0:9d06cdd73bc7, 18 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?

UserRevisionLine numberNew 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