CANSAT_AIRFUL / Mbed 2 deprecated mbed_paper_exponential

step3

Dependencies:   Servo mbed

Fork of mbed_main by CANSAT_AIRFUL

main.cpp@4:3ed1a684a383, 2016-11-19 (annotated)

Committer:
Soyoon
Date:
Sat Nov 19 15:05:43 2016 +0000
Revision:
4:3ed1a684a383
Parent:
3:e3e965924dde 
3; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soyoon 0:6ac6b2d2bf1a 1 #include "mbed.h"
Soyoon 0:6ac6b2d2bf1a 2 #include "Barometer.h"
Soyoon 1:cd11c1c592c7 3 #include "LocalFileSystem.h"
Soyoon 0:6ac6b2d2bf1a 4 #include "math.h"
Soyoon 2:9d0f979369cf 5 #include "Servo.h"
Soyoon 4:3ed1a684a383 6 #define dt 0.1
Soyoon 1:cd11c1c592c7 7
Soyoon 4:3ed1a684a383 8 Serial pc(USBTX, USBRX);
Soyoon 3:e3e965924dde 9 Ticker blue_trig;
Soyoon 2:9d0f979369cf 10 Timer end;
Soyoon 0:6ac6b2d2bf1a 11
Soyoon 4:3ed1a684a383 12 ////////////////////////////////////////
Soyoon 4:3ed1a684a383 13 // Bluetooth 3.3V p13 p14(TX,RX) //
Soyoon 4:3ed1a684a383 14 ////////////////////////////////////////
Soyoon 4:3ed1a684a383 15 Serial Blue(p13, p14);
Soyoon 4:3ed1a684a383 16 int send_ok=0;
Soyoon 4:3ed1a684a383 17 char Blue_msg[150];
Soyoon 4:3ed1a684a383 18 int k=0, Blue_ok=0, Blue_flag=0;
Soyoon 4:3ed1a684a383 19 volatile unsigned char Blue_buffer[2];
Soyoon 4:3ed1a684a383 20 float input_roll, input_pitch, input_yaw, input_thr;
Soyoon 0:6ac6b2d2bf1a 21
Soyoon 4:3ed1a684a383 22 void Blue_isr(){ //inturupt
Soyoon 4:3ed1a684a383 23 while(Blue.readable()){
Soyoon 4:3ed1a684a383 24 Blue_buffer[1] = Blue_buffer[0];
Soyoon 4:3ed1a684a383 25 Blue_buffer[0] = Blue.getc();
Soyoon 4:3ed1a684a383 26 if (Blue_buffer[0] == '\n' && Blue_flag == 1){Blue_flag = 0; Blue_ok = 1; k=0;}
Soyoon 4:3ed1a684a383 27 if (Blue_buffer[0] == '*'){Blue_flag=1;}
Soyoon 4:3ed1a684a383 28 if (Blue_flag==1){Blue_msg[k] = Blue_buffer[0]; k++;}
Soyoon 1:cd11c1c592c7 29 }
Soyoon 4:3ed1a684a383 30 }
Soyoon 0:6ac6b2d2bf1a 31
Soyoon 4:3ed1a684a383 32 void get_Blue(float*input_roll, float*input_pitch, float*input_yaw, float*input_thr)
Soyoon 1:cd11c1c592c7 33 {
Soyoon 4:3ed1a684a383 34 if (Blue_ok == 1){
Soyoon 4:3ed1a684a383 35 Blue_ok = 0;
Soyoon 4:3ed1a684a383 36 sscanf(Blue_msg, "*%f,%f,%f,%f\n", input_roll, input_pitch, input_yaw, input_thr);
Soyoon 1:cd11c1c592c7 37 }
Soyoon 4:3ed1a684a383 38 }
Soyoon 1:cd11c1c592c7 39
Soyoon 3:e3e965924dde 40 void blue_trig_isr(){
Soyoon 4:3ed1a684a383 41 send_ok=1;
Soyoon 3:e3e965924dde 42 }
Soyoon 3:e3e965924dde 43
Soyoon 3:e3e965924dde 44 void trans_blue_data(float in_data, int integer_point, int under_point){ // number of intefer and under_point
Soyoon 3:e3e965924dde 45 unsigned int conv_trans_data;
Soyoon 3:e3e965924dde 46 conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point));
Soyoon 4:3ed1a684a383 47 if(in_data<0) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '-';}
Soyoon 4:3ed1a684a383 48 else {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '+';}
Soyoon 3:e3e965924dde 49 for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
Soyoon 4:3ed1a684a383 50 if(cnt_num == under_point) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '.'; }
Soyoon 4:3ed1a684a383 51 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48); //convert to ASCII
Soyoon 3:e3e965924dde 52 }
Soyoon 3:e3e965924dde 53 }
Soyoon 3:e3e965924dde 54
Soyoon 2:9d0f979369cf 55 //Bluetooth code is placed under the Log_data
Soyoon 2:9d0f979369cf 56
Soyoon 2:9d0f979369cf 57 ////////////////////////////////////////////
Soyoon 2:9d0f979369cf 58 // Barometer 3.3V p9(SDA) p10(SCL) //
Soyoon 2:9d0f979369cf 59 ////////////////////////////////////////////
Soyoon 2:9d0f979369cf 60 Barometer barometer(p9, p10);
Soyoon 4:3ed1a684a383 61 float alt=0.0;
Soyoon 2:9d0f979369cf 62 float alt_sum=0.0f, alt_zero=0.0f;
Soyoon 2:9d0f979369cf 63 int count = 0, baro_ok = 0; // for zero-calibration
Soyoon 4:3ed1a684a383 64 float alt_buffer[2], w_alt=0; // weight for LPF
Soyoon 2:9d0f979369cf 65
Soyoon 4:3ed1a684a383 66 void get_Baro(float*alt)
Soyoon 2:9d0f979369cf 67 {
Soyoon 2:9d0f979369cf 68 if (baro_ok==1){
Soyoon 3:e3e965924dde 69 barometer.update();
Soyoon 3:e3e965924dde 70 *alt = barometer.get_altitude_m();
Soyoon 3:e3e965924dde 71 alt_buffer[1] = alt_buffer[0];
Soyoon 3:e3e965924dde 72 alt_buffer[0] = *alt;
Soyoon 4:3ed1a684a383 73 if(abs(alt_buffer[0]- alt_buffer[1])>20){
Soyoon 4:3ed1a684a383 74 *alt = alt_buffer[1];
Soyoon 3:e3e965924dde 75 baro_ok = 0;
Soyoon 3:e3e965924dde 76 }
Soyoon 3:e3e965924dde 77 else{
Soyoon 3:e3e965924dde 78 baro_ok = 0;
Soyoon 3:e3e965924dde 79 }
Soyoon 2:9d0f979369cf 80 }
Soyoon 2:9d0f979369cf 81 }
Soyoon 2:9d0f979369cf 82
Soyoon 2:9d0f979369cf 83 void calb_alt(){
Soyoon 3:e3e965924dde 84 if (alt==0){count=0;}
Soyoon 2:9d0f979369cf 85 else {
Soyoon 3:e3e965924dde 86 if (count==1){count++;}
Soyoon 3:e3e965924dde 87 else{
Soyoon 3:e3e965924dde 88 if (count<=99){alt_sum = alt_sum + alt; count++;}
Soyoon 4:3ed1a684a383 89 else {alt_zero = alt_sum/(float)(count-1); count++;}
Soyoon 2:9d0f979369cf 90 }
Soyoon 2:9d0f979369cf 91 }
Soyoon 2:9d0f979369cf 92 }
Soyoon 2:9d0f979369cf 93
Soyoon 1:cd11c1c592c7 94 ///////////////////////////////////////
Soyoon 4:3ed1a684a383 95 // AHRS 5V p27(RX) // 20Hz
Soyoon 1:cd11c1c592c7 96 ///////////////////////////////////////
Soyoon 4:3ed1a684a383 97 Serial AHRS(p28, p27);
Soyoon 4:3ed1a684a383 98 float roll,pitch,yaw,velx,vely,velz,velxyz;
Soyoon 2:9d0f979369cf 99 char AHRS_msg[150];
Soyoon 3:e3e965924dde 100 int m=0, ahrs_ok=0, AHRS_flag=0;
Soyoon 2:9d0f979369cf 101 volatile unsigned char AHRS_buffer[2];
Soyoon 2:9d0f979369cf 102
Soyoon 2:9d0f979369cf 103 void AHRS_isr(){ //inturupt
Soyoon 2:9d0f979369cf 104 while(AHRS.readable()){
Soyoon 2:9d0f979369cf 105 AHRS_buffer[1] = AHRS_buffer[0];
Soyoon 2:9d0f979369cf 106 AHRS_buffer[0] = AHRS.getc();
Soyoon 2:9d0f979369cf 107 if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
Soyoon 2:9d0f979369cf 108 if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
Soyoon 2:9d0f979369cf 109 if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
Soyoon 2:9d0f979369cf 110 }
Soyoon 2:9d0f979369cf 111 }
Soyoon 1:cd11c1c592c7 112
Soyoon 4:3ed1a684a383 113 void get_AHRS(float*roll, float*pitch, float*yaw, float*velx, float*vely, float*velz, float*velxyz)
Soyoon 1:cd11c1c592c7 114 {
Soyoon 2:9d0f979369cf 115 if (ahrs_ok == 1){
Soyoon 2:9d0f979369cf 116 ahrs_ok = 0;
Soyoon 4:3ed1a684a383 117 sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, velx, vely, velz);
Soyoon 4:3ed1a684a383 118 *velxyz = (float)sqrt(pow(*velx,2)+pow(*vely,2)+pow(*velz,2));
Soyoon 2:9d0f979369cf 119 baro_ok = 1;
Soyoon 2:9d0f979369cf 120 }
Soyoon 3:e3e965924dde 121 }
Soyoon 1:cd11c1c592c7 122
Soyoon 1:cd11c1c592c7 123 ///////////////////////////////////
Soyoon 4:3ed1a684a383 124 // Servo 5V PWM // needed to check pin#
Soyoon 1:cd11c1c592c7 125 ///////////////////////////////////
Soyoon 4:3ed1a684a383 126 Servo Throttle(p26);
Soyoon 4:3ed1a684a383 127 Servo CS1(p25); //below
Soyoon 4:3ed1a684a383 128 Servo CS2(p23); //upper
Soyoon 4:3ed1a684a383 129 Servo CS3(p21); //below
Soyoon 4:3ed1a684a383 130 Servo CS4(p22); //upper
Soyoon 4:3ed1a684a383 131
Soyoon 4:3ed1a684a383 132 float thr_value = 0.0, ctrl1_value = 0.5, ctrl2_value = 0.5, ctrl3_value = 0.5, ctrl4_value = 0.5;
Soyoon 4:3ed1a684a383 133 float err_roll = 0.0, err_pitch = 0.0, err_yaw = 0.0, err_alt = 0.0, preerror_roll = 0.0, preerror_pitch = 0.0, preerror_yaw = 0.0;
Soyoon 4:3ed1a684a383 134 float ctrl_roll = 0.0, ctrl_pitch = 0.0, ctrl_yaw = 0.0;
Soyoon 4:3ed1a684a383 135 float kp1=1.0, kd1=0.0, kp2=1.0, kd2=0.0, kp3=1.0, kd3=0.0; // PD Controller
Soyoon 2:9d0f979369cf 136
Soyoon 4:3ed1a684a383 137 void ctrl_attitude(){
Soyoon 4:3ed1a684a383 138 err_roll = input_roll - roll;
Soyoon 4:3ed1a684a383 139 err_pitch = input_pitch - pitch;
Soyoon 4:3ed1a684a383 140 err_yaw = input_yaw - yaw;
Soyoon 4:3ed1a684a383 141 ctrl_roll = ((kp1*err_roll)+(kd1*(err_roll-preerror_roll)/dt))/180.0;
Soyoon 4:3ed1a684a383 142 ctrl_pitch = ((kp2*err_pitch)+(kd2*(err_pitch-preerror_pitch)/dt))/180.0;
Soyoon 4:3ed1a684a383 143 ctrl_yaw = ((kp3*err_yaw)+(kd3*(err_yaw-preerror_yaw)/dt))/180.0; // PD control and Range adjustment
Soyoon 4:3ed1a684a383 144 preerror_roll = err_roll;
Soyoon 4:3ed1a684a383 145 preerror_pitch = err_pitch;
Soyoon 4:3ed1a684a383 146 preerror_yaw = err_yaw;
Soyoon 4:3ed1a684a383 147
Soyoon 4:3ed1a684a383 148 // Roll = ctrl2 - ctrl4 Controll surface movement, Servo motor forwards same direction
Soyoon 4:3ed1a684a383 149 // Pitch = ctrl1 - ctrl3
Soyoon 4:3ed1a684a383 150 // Yaw = ctrl1 + ctrl2 + ctrl3 + ctrl4
Soyoon 4:3ed1a684a383 151
Soyoon 4:3ed1a684a383 152 //Servo1 Control (forward)
Soyoon 4:3ed1a684a383 153 ctrl1_value = ((ctrl_pitch/2.0) + (ctrl_yaw/4.0));
Soyoon 4:3ed1a684a383 154 if (ctrl1_value<0){ctrl1_value = 1.0-0.5*exp(0.0039*(-ctrl1_value));}
Soyoon 4:3ed1a684a383 155 else {ctrl1_value = 0.5*exp(0.0039*ctrl1_value);}
Soyoon 4:3ed1a684a383 156 if (ctrl1_value<=0.0){ctrl1_value=0.0;}
Soyoon 4:3ed1a684a383 157 else if (ctrl1_value>=1.0) {ctrl1_value=1.0;}
Soyoon 4:3ed1a684a383 158
Soyoon 4:3ed1a684a383 159 //Servo2 Control (right)
Soyoon 4:3ed1a684a383 160 ctrl2_value = ((ctrl_roll/2.0) + (ctrl_yaw/4.0));
Soyoon 4:3ed1a684a383 161 if (ctrl2_value<0){ctrl2_value = 1.0-0.5*exp(0.0039*(-ctrl2_value));}
Soyoon 4:3ed1a684a383 162 else {ctrl2_value = 0.5*exp(0.0039*ctrl2_value);}
Soyoon 4:3ed1a684a383 163 if (ctrl2_value<=0.0){ctrl2_value=0.0;}
Soyoon 4:3ed1a684a383 164 else if (ctrl2_value>=1.0) {ctrl2_value=1.0;}
Soyoon 4:3ed1a684a383 165
Soyoon 4:3ed1a684a383 166 //Servo3 Control (backward)
Soyoon 4:3ed1a684a383 167 ctrl3_value = (-(ctrl_pitch/2.0) + (ctrl_yaw/4.0));
Soyoon 4:3ed1a684a383 168 if (ctrl3_value<0){ctrl3_value = 1.0-0.5*exp(0.0039*(-ctrl3_value));}
Soyoon 4:3ed1a684a383 169 else {ctrl3_value = 0.5*exp(0.0039*ctrl3_value);}
Soyoon 4:3ed1a684a383 170 if (ctrl3_value<=0.0){ctrl3_value=0.0;}
Soyoon 4:3ed1a684a383 171 else if (ctrl3_value>=1.0) {ctrl3_value=1.0;}
Soyoon 4:3ed1a684a383 172
Soyoon 4:3ed1a684a383 173 //Servo4 Control (left)
Soyoon 4:3ed1a684a383 174 ctrl4_value = (-(ctrl_roll/2.0) + (ctrl_yaw/4.0));
Soyoon 4:3ed1a684a383 175 if (ctrl4_value<0){ctrl4_value = 1.0-0.5*exp(0.0039*(-ctrl4_value));}
Soyoon 4:3ed1a684a383 176 else {ctrl4_value = 0.5*exp(0.0039*ctrl4_value);}
Soyoon 4:3ed1a684a383 177 if (ctrl4_value<=0.0){ctrl4_value=0.0;}
Soyoon 4:3ed1a684a383 178 else if (ctrl4_value>=1.0) {ctrl4_value=1.0;}
Soyoon 4:3ed1a684a383 179
Soyoon 4:3ed1a684a383 180 CS1 = ctrl1_value;
Soyoon 4:3ed1a684a383 181 CS2 = ctrl2_value;
Soyoon 4:3ed1a684a383 182 CS3 = ctrl3_value;
Soyoon 4:3ed1a684a383 183 CS4 = ctrl4_value;
Soyoon 4:3ed1a684a383 184 }
Soyoon 2:9d0f979369cf 185
Soyoon 4:3ed1a684a383 186
Soyoon 4:3ed1a684a383 187 void neutral(){
Soyoon 4:3ed1a684a383 188 ctrl1_value = 0.5;
Soyoon 4:3ed1a684a383 189 ctrl2_value = 0.5;
Soyoon 4:3ed1a684a383 190 ctrl3_value = 0.5;
Soyoon 4:3ed1a684a383 191 ctrl4_value = 0.5;
Soyoon 4:3ed1a684a383 192 CS1 = ctrl1_value;
Soyoon 4:3ed1a684a383 193 CS2 = ctrl2_value;
Soyoon 4:3ed1a684a383 194 CS3 = ctrl3_value;
Soyoon 4:3ed1a684a383 195 CS4 = ctrl4_value;
Soyoon 2:9d0f979369cf 196 }
Soyoon 1:cd11c1c592c7 197
Soyoon 1:cd11c1c592c7 198 ///////////////////////////////
Soyoon 1:cd11c1c592c7 199 // Datalogger Mbed //
Soyoon 1:cd11c1c592c7 200 ///////////////////////////////
Soyoon 1:cd11c1c592c7 201 LocalFileSystem local("local");
Soyoon 2:9d0f979369cf 202 int file_no=1;
Soyoon 2:9d0f979369cf 203 char filename[256];
Soyoon 1:cd11c1c592c7 204 FILE *fp;
Soyoon 1:cd11c1c592c7 205
Soyoon 2:9d0f979369cf 206 void Log_file(){
Soyoon 2:9d0f979369cf 207 sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
Soyoon 2:9d0f979369cf 208 fp = fopen(filename, "r"); // if file can be loaded, return is 1
Soyoon 2:9d0f979369cf 209 while(fp){
Soyoon 2:9d0f979369cf 210 fclose(fp);
Soyoon 2:9d0f979369cf 211 file_no ++;
Soyoon 2:9d0f979369cf 212 sprintf(filename, "/local/Data%d.txt", file_no); // Open "tem%d.txt" on the local file system for writing
Soyoon 2:9d0f979369cf 213 fp = fopen(filename, "r");
Soyoon 2:9d0f979369cf 214 }
Soyoon 2:9d0f979369cf 215 fp = fopen(filename, "w");
Soyoon 2:9d0f979369cf 216 }
Soyoon 2:9d0f979369cf 217
Soyoon 1:cd11c1c592c7 218 void Log_data(){
Soyoon 4:3ed1a684a383 219 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",roll,pitch,yaw,alt,input_roll,input_pitch,input_yaw,input_thr,velx,vely,velz);
Soyoon 1:cd11c1c592c7 220 }
Soyoon 1:cd11c1c592c7 221
Soyoon 2:9d0f979369cf 222 void send_Blue(){
Soyoon 4:3ed1a684a383 223 if (send_ok == 1){
Soyoon 4:3ed1a684a383 224 send_ok = 0;
Soyoon 4:3ed1a684a383 225 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '*';
Soyoon 3:e3e965924dde 226 trans_blue_data(roll,3,2);
Soyoon 4:3ed1a684a383 227 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 3:e3e965924dde 228 trans_blue_data(pitch,3,2);
Soyoon 4:3ed1a684a383 229 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 3:e3e965924dde 230 trans_blue_data(yaw,3,2);
Soyoon 4:3ed1a684a383 231 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 4:3ed1a684a383 232 trans_blue_data(alt,2,2);
Soyoon 4:3ed1a684a383 233 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 4:3ed1a684a383 234 trans_blue_data(velxyz,2,2);
Soyoon 4:3ed1a684a383 235 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 4:3ed1a684a383 236 trans_blue_data(velz,2,2);
Soyoon 4:3ed1a684a383 237 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '\n';
Soyoon 3:e3e965924dde 238 }
Soyoon 2:9d0f979369cf 239 }
Soyoon 2:9d0f979369cf 240
Soyoon 3:e3e965924dde 241
Soyoon 2:9d0f979369cf 242 /////////////////////////////////
Soyoon 3:e3e965924dde 243 // Main loop //
Soyoon 2:9d0f979369cf 244 /////////////////////////////////
Soyoon 2:9d0f979369cf 245
Soyoon 0:6ac6b2d2bf1a 246 int main(void)
Soyoon 1:cd11c1c592c7 247 {
Soyoon 1:cd11c1c592c7 248 AHRS.baud(9600);
Soyoon 4:3ed1a684a383 249 Blue.baud(9600);
Soyoon 4:3ed1a684a383 250 Blue.attach(&Blue_isr);
Soyoon 3:e3e965924dde 251 AHRS.attach(&AHRS_isr);
Soyoon 4:3ed1a684a383 252 blue_trig.attach(&blue_trig_isr, 0.1);
Soyoon 4:3ed1a684a383 253 neutral();
Soyoon 4:3ed1a684a383 254 Log_file();
Soyoon 1:cd11c1c592c7 255 while(1) {
Soyoon 4:3ed1a684a383 256 get_AHRS(&roll,&pitch,&yaw,&velx,&vely,&velz,&velxyz);
Soyoon 4:3ed1a684a383 257 get_Blue(&input_roll,&input_pitch,&input_yaw,&input_thr);
Soyoon 4:3ed1a684a383 258 get_Baro(&alt);
Soyoon 4:3ed1a684a383 259 if (input_roll==0){input_roll = roll;}
Soyoon 4:3ed1a684a383 260 if (input_pitch==0){input_pitch = pitch;}
Soyoon 4:3ed1a684a383 261 if (input_yaw==0){input_yaw = yaw;}
Soyoon 4:3ed1a684a383 262 ctrl_attitude();
Soyoon 2:9d0f979369cf 263 Log_data();
Soyoon 2:9d0f979369cf 264 send_Blue();
Soyoon 4:3ed1a684a383 265 if (input_roll==180.0 && input_pitch==180.0 && input_yaw==180.0 && input_thr==0){fclose(fp); break;}
Soyoon 1:cd11c1c592c7 266 }
Soyoon 1:cd11c1c592c7 267 }