Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ESC FreeIMU mbed-rtos mbed

main.cpp@2:7439607ccd51, 2013-10-15 (annotated)

Committer:
MatteoT
Date:
Tue Oct 15 18:30:46 2013 +0000
Revision:
2:7439607ccd51
Parent:
1:acb2e0f9d1bc
Child:
3:5e7476bca25f
IMU experiments with serial communication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 2:7439607ccd51 1 #include "math.h"
MatteoT 0:22b18227d29e 2 #include "mbed.h"
MatteoT 2:7439607ccd51 3 #include "rtos.h"
MatteoT 2:7439607ccd51 4 //#include "EthernetInterface.h"
MatteoT 2:7439607ccd51 5 //#include "RefRX.h"
MatteoT 0:22b18227d29e 6 #include "esc.h"
MatteoT 2:7439607ccd51 7 #include "state.h"
MatteoT 2:7439607ccd51 8 #include "TXRX.h"
MatteoT 2:7439607ccd51 9 #include "IMU.h"
MatteoT 2:7439607ccd51 10 #include "PID.h"
MatteoT 0:22b18227d29e 11
MatteoT 0:22b18227d29e 12 int main()
MatteoT 0:22b18227d29e 13 {
MatteoT 0:22b18227d29e 14 //Setup ESCs
MatteoT 2:7439607ccd51 15 ESC esc1(p23);
MatteoT 2:7439607ccd51 16 ESC esc2(p24);
MatteoT 2:7439607ccd51 17 ESC esc3(p25);
MatteoT 2:7439607ccd51 18 ESC esc4(p26);
MatteoT 2:7439607ccd51 19
MatteoT 2:7439607ccd51 20 //Setup status leds
MatteoT 2:7439607ccd51 21 DigitalOut led1(LED1); led1 = 1; //esc
MatteoT 2:7439607ccd51 22 DigitalOut led4(LED4); led4 = 0; //signal
MatteoT 2:7439607ccd51 23 //DigitalOut led3(LED3); led3 = 0; //tmp signal send
MatteoT 2:7439607ccd51 24 //DigitalOut led2(LED2); led2 = 0;//moved to IMU.h
MatteoT 2:7439607ccd51 25
MatteoT 2:7439607ccd51 26 //Setup Ethernet
MatteoT 2:7439607ccd51 27 //EthernetInterface interface;
MatteoT 2:7439607ccd51 28 //interface.init("192.168.1.16","255.255.255.0","192.168.1.1");
MatteoT 2:7439607ccd51 29 //interface.connect();
MatteoT 0:22b18227d29e 30
MatteoT 0:22b18227d29e 31 //Setup RefRX static class for remote control
MatteoT 2:7439607ccd51 32 //Thread RefRX_thread (RefRX::worker);
MatteoT 2:7439607ccd51 33 //RefRX::init(2222, 2223, RefRX_thread);
MatteoT 2:7439607ccd51 34 //Serial pc(USBTX, USBRX);
MatteoT 2:7439607ccd51 35 //pc.baud(115200);
MatteoT 2:7439607ccd51 36 //pc.printf("Serial setted up\r\n");
MatteoT 2:7439607ccd51 37 Thread TXRX_thread (TXRX_thread_routine);
MatteoT 2:7439607ccd51 38 led4 = 1;
MatteoT 2:7439607ccd51 39 //Thread::wait(0.5);
MatteoT 2:7439607ccd51 40
MatteoT 2:7439607ccd51 41 //Setup FreeIMU
MatteoT 2:7439607ccd51 42 //FreeIMU freeIMU;
MatteoT 2:7439607ccd51 43 //freeIMU.init();
MatteoT 2:7439607ccd51 44 Thread IMU_thread (IMU_thread_routine);
MatteoT 2:7439607ccd51 45 //pc.printf("IMU thread started\r\n");
MatteoT 0:22b18227d29e 46
MatteoT 2:7439607ccd51 47 //Setup state
MatteoT 2:7439607ccd51 48 QuadState mainQuadState; mainQuadState.reset();
MatteoT 2:7439607ccd51 49 QuadState rxQuadState; rxQuadState.reset();
MatteoT 2:7439607ccd51 50 QuadState txQuadState; txQuadState.reset();
MatteoT 2:7439607ccd51 51 //pc.printf("Main and rx states setted up\r\n");
MatteoT 2:7439607ccd51 52
MatteoT 2:7439607ccd51 53 //Prepare controll loop
MatteoT 2:7439607ccd51 54 //float ref1=0, ref2=0, ref3=0, ref4=0;
MatteoT 2:7439607ccd51 55 double dt=0.001; //will be measured
MatteoT 2:7439607ccd51 56 Timer dt_t;
MatteoT 2:7439607ccd51 57 dt_t.start();
MatteoT 2:7439607ccd51 58 //unsigned int state_read_i=0;
MatteoT 2:7439607ccd51 59 //unsigned int state_send_i=0;
MatteoT 2:7439607ccd51 60
MatteoT 2:7439607ccd51 61 //pid
MatteoT 2:7439607ccd51 62 PID targetRoll_pid(0.1,0.0001,0.5, 0.020);
MatteoT 2:7439607ccd51 63 targetRoll_pid.setInputLimits(-3.15/4,3.15/4);
MatteoT 2:7439607ccd51 64 targetRoll_pid.setOutputLimits(0,0.5);
MatteoT 2:7439607ccd51 65
MatteoT 2:7439607ccd51 66 //pc.printf("Now going to start main loop...\r\n");
MatteoT 0:22b18227d29e 67
MatteoT 2:7439607ccd51 68 led1 = !led1;
MatteoT 0:22b18227d29e 69 //Begins main control loop
MatteoT 2:7439607ccd51 70 while(1)
MatteoT 2:7439607ccd51 71 {
MatteoT 2:7439607ccd51 72 //retrive refs
MatteoT 2:7439607ccd51 73 //led4 ^= RefRX::get(ref1,ref2,ref3,ref4); //flip-flop-flip-flop!
MatteoT 2:7439607ccd51 74 /*
MatteoT 2:7439607ccd51 75 if(pc.readable()){
MatteoT 2:7439607ccd51 76 *((unsigned char *)(&rxQuadState) + state_read_i) = pc.getc();
MatteoT 2:7439607ccd51 77 ++state_read_i;
MatteoT 2:7439607ccd51 78 if(state_read_i >= sizeof(QuadState))
MatteoT 2:7439607ccd51 79 state_read_i = 0;
MatteoT 2:7439607ccd51 80 led4 = 1;
MatteoT 2:7439607ccd51 81 }
MatteoT 2:7439607ccd51 82 else
MatteoT 2:7439607ccd51 83 led4 = 0;
MatteoT 2:7439607ccd51 84 */
MatteoT 2:7439607ccd51 85 unsigned int rx_delta, rx_wait;
MatteoT 2:7439607ccd51 86 if(TXRX_stateExchange(txQuadState,rxQuadState))
MatteoT 2:7439607ccd51 87 if(rxQuadState.actual_rx_delta_ms >= 5000 || rxQuadState.actual_rx_wait_ms >= 5000)
MatteoT 2:7439607ccd51 88 mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER = true; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
MatteoT 2:7439607ccd51 89
MatteoT 2:7439607ccd51 90 //retrive inertial values
MatteoT 2:7439607ccd51 91 IMU_filter_getYawPitchRoll(
MatteoT 2:7439607ccd51 92 mainQuadState.estimated_rotation_y,
MatteoT 2:7439607ccd51 93 mainQuadState.estimated_rotation_p,
MatteoT 2:7439607ccd51 94 mainQuadState.estimated_rotation_r );
MatteoT 2:7439607ccd51 95
MatteoT 2:7439607ccd51 96 //calc dt
MatteoT 2:7439607ccd51 97 dt_t.stop();
MatteoT 2:7439607ccd51 98 dt = (double) (1.0/1000000.0) * dt_t.read_us();
MatteoT 2:7439607ccd51 99 dt_t.reset(); dt_t.start();
MatteoT 2:7439607ccd51 100 mainQuadState.average_throttle_dt += dt * mainQuadState.average_throttle_dt_k;
MatteoT 2:7439607ccd51 101 mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
MatteoT 2:7439607ccd51 102 mainQuadState.actual_throttle_dt = dt;
MatteoT 0:22b18227d29e 103
MatteoT 2:7439607ccd51 104 //elaborate signals
MatteoT 2:7439607ccd51 105 if(mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER){
MatteoT 2:7439607ccd51 106 mainQuadState.actual_throttle_1 = 0;
MatteoT 2:7439607ccd51 107 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 108 mainQuadState.actual_throttle_3 = 0;
MatteoT 2:7439607ccd51 109 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 110 }else{
MatteoT 2:7439607ccd51 111 mainQuadState.actual_rx_delta_ms = rxQuadState.actual_rx_delta_ms;
MatteoT 2:7439607ccd51 112 mainQuadState.actual_rx_wait_ms = rxQuadState.actual_rx_wait_ms;
MatteoT 2:7439607ccd51 113 mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
MatteoT 2:7439607ccd51 114 mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
MatteoT 2:7439607ccd51 115
MatteoT 2:7439607ccd51 116
MatteoT 2:7439607ccd51 117 float roll = 0;
MatteoT 2:7439607ccd51 118 if(mainQuadState.target_rotation_r_isRequired){
MatteoT 2:7439607ccd51 119 targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
MatteoT 2:7439607ccd51 120 targetRoll_pid.setInterval(mainQuadState.average_throttle_dt);
MatteoT 2:7439607ccd51 121 targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
MatteoT 2:7439607ccd51 122 roll = targetRoll_pid.compute();
MatteoT 2:7439607ccd51 123 }else{
MatteoT 2:7439607ccd51 124 targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
MatteoT 2:7439607ccd51 125 }
MatteoT 2:7439607ccd51 126
MatteoT 2:7439607ccd51 127 if(rxQuadState.requested_throttle_1 >= 0 && rxQuadState.requested_throttle_1 <=1)
MatteoT 2:7439607ccd51 128 mainQuadState.actual_throttle_1 = rxQuadState.requested_throttle_1;
MatteoT 2:7439607ccd51 129 else{
MatteoT 2:7439607ccd51 130 float out = 0.5; //NOTE: here we will have full throttle calcs..
MatteoT 2:7439607ccd51 131 out -= roll;
MatteoT 2:7439607ccd51 132 mainQuadState.actual_throttle_1 = out;
MatteoT 2:7439607ccd51 133 }
MatteoT 2:7439607ccd51 134 if(rxQuadState.requested_throttle_2 >= 0 && rxQuadState.requested_throttle_2 <=1)
MatteoT 2:7439607ccd51 135 mainQuadState.actual_throttle_2 = rxQuadState.requested_throttle_2;
MatteoT 2:7439607ccd51 136 else{
MatteoT 2:7439607ccd51 137 mainQuadState.actual_throttle_2 = 0;
MatteoT 2:7439607ccd51 138 }
MatteoT 2:7439607ccd51 139 if(rxQuadState.requested_throttle_3 >= 0 && rxQuadState.requested_throttle_3 <=1)
MatteoT 2:7439607ccd51 140 mainQuadState.actual_throttle_3 = rxQuadState.requested_throttle_3;
MatteoT 2:7439607ccd51 141 else{
MatteoT 2:7439607ccd51 142 float out = 0.5;
MatteoT 2:7439607ccd51 143 out += roll;
MatteoT 2:7439607ccd51 144 mainQuadState.actual_throttle_3 = out;
MatteoT 2:7439607ccd51 145 }
MatteoT 2:7439607ccd51 146 if(rxQuadState.requested_throttle_4 >= 0 && rxQuadState.requested_throttle_4 <=1)
MatteoT 2:7439607ccd51 147 mainQuadState.actual_throttle_4 = rxQuadState.requested_throttle_4;
MatteoT 2:7439607ccd51 148 else{
MatteoT 2:7439607ccd51 149 mainQuadState.actual_throttle_4 = 0;
MatteoT 2:7439607ccd51 150 }
MatteoT 2:7439607ccd51 151 }
MatteoT 0:22b18227d29e 152
MatteoT 0:22b18227d29e 153 //compute new ESC values..
MatteoT 2:7439607ccd51 154 esc1 = mainQuadState.actual_throttle_1;
MatteoT 2:7439607ccd51 155 esc2 = mainQuadState.actual_throttle_2;
MatteoT 2:7439607ccd51 156 esc3 = mainQuadState.actual_throttle_3;
MatteoT 2:7439607ccd51 157 esc4 = mainQuadState.actual_throttle_4;
MatteoT 0:22b18227d29e 158
MatteoT 0:22b18227d29e 159 //finally output to ESCs
MatteoT 1:acb2e0f9d1bc 160 esc1();
MatteoT 1:acb2e0f9d1bc 161 esc2();
MatteoT 1:acb2e0f9d1bc 162 esc3();
MatteoT 1:acb2e0f9d1bc 163 esc4();
MatteoT 0:22b18227d29e 164
MatteoT 2:7439607ccd51 165 led1 = !led1;
MatteoT 2:7439607ccd51 166
MatteoT 2:7439607ccd51 167
MatteoT 2:7439607ccd51 168 uint32_t w = 20-dt;
MatteoT 2:7439607ccd51 169 if(w<5) w=5;
MatteoT 2:7439607ccd51 170 else if(w>19) w=19;
MatteoT 2:7439607ccd51 171 Thread::wait(w /*ms*/);
MatteoT 2:7439607ccd51 172
MatteoT 2:7439607ccd51 173
MatteoT 2:7439607ccd51 174 //send info back
MatteoT 2:7439607ccd51 175 txQuadState = mainQuadState;
MatteoT 2:7439607ccd51 176
MatteoT 2:7439607ccd51 177 /*
MatteoT 2:7439607ccd51 178 for(int i=0; i<sizeof(QuadState); ++i){
MatteoT 2:7439607ccd51 179 while(!pc.writeable())
MatteoT 2:7439607ccd51 180 led3=0;
MatteoT 2:7439607ccd51 181 pc.putc((int) *((unsigned char *) (&mainQuadState) + i));
MatteoT 2:7439607ccd51 182 }*/
MatteoT 2:7439607ccd51 183 /*
MatteoT 2:7439607ccd51 184 if(state_send_i >= 7){
MatteoT 2:7439607ccd51 185 state_send_i = 0;
MatteoT 2:7439607ccd51 186 time_t sec = time(NULL);
MatteoT 2:7439607ccd51 187 pc.printf("\033[2J\033[1;1H");
MatteoT 2:7439607ccd51 188 pc.printf("Time: %s", ctime(&sec));
MatteoT 2:7439607ccd51 189 pc.printf("Actual thrust: \t\t %8.2f %8.2f %8.2f %8.2f\n",
MatteoT 2:7439607ccd51 190 mainQuadState.actual_throttle_1, mainQuadState.actual_throttle_2,
MatteoT 2:7439607ccd51 191 mainQuadState.actual_throttle_3, mainQuadState.actual_throttle_4 );
MatteoT 2:7439607ccd51 192 pc.printf("IMU yaw pitch roll: \t %10.1f° %10.1f° %10.1f°\n", mainQuadState.estimated_rotation_y*180.0/3.141592653589793,
MatteoT 2:7439607ccd51 193 mainQuadState.estimated_rotation_p*180.0/3.141592653589793, mainQuadState.estimated_rotation_r*180.0/3.141592653589793 );
MatteoT 2:7439607ccd51 194 pc.printf("Avg/actual delta time: \t %10.2f ms %10.2f ms \n", mainQuadState.average_throttle_dt*1000.0, mainQuadState.actual_throttle_dt*1000.0);
MatteoT 2:7439607ccd51 195 led3=pc.writeable();
MatteoT 2:7439607ccd51 196 }
MatteoT 2:7439607ccd51 197 ++state_send_i;*/
MatteoT 0:22b18227d29e 198 }
MatteoT 2:7439607ccd51 199
MatteoT 2:7439607ccd51 200 return 0;
MatteoT 0:22b18227d29e 201 }