CANSAT_AIRFUL / Mbed 2 deprecated mbed_paper_mixing

mixing control

Dependencies:   Servo mbed

Fork of mbed_main by CANSAT_AIRFUL

main.cpp@4:4324f20e4597, 2016-11-19 (annotated)

Committer:
Soyoon
Date:
Sat Nov 19 15:05:11 2016 +0000
Revision:
4:4324f20e4597
Parent:
3:e3e965924dde 
2;

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:4324f20e4597 6 #define dt 0.1
Soyoon 1:cd11c1c592c7 7
Soyoon 4:4324f20e4597 8 Serial pc(USBTX, USBRX);
Soyoon 3:e3e965924dde 9 Ticker blue_trig;
Soyoon 2:9d0f979369cf 10 Timer end;
Soyoon 0:6ac6b2d2bf1a 11
Soyoon 4:4324f20e4597 12 ////////////////////////////////////////
Soyoon 4:4324f20e4597 13 // Bluetooth 3.3V p13 p14(TX,RX) //
Soyoon 4:4324f20e4597 14 ////////////////////////////////////////
Soyoon 4:4324f20e4597 15 Serial Blue(p13, p14);
Soyoon 4:4324f20e4597 16 int send_ok=0;
Soyoon 4:4324f20e4597 17 char Blue_msg[150];
Soyoon 4:4324f20e4597 18 int k=0, Blue_ok=0, Blue_flag=0;
Soyoon 4:4324f20e4597 19 volatile unsigned char Blue_buffer[2];
Soyoon 4:4324f20e4597 20 float input_roll, input_pitch, input_yaw, input_thr;
Soyoon 0:6ac6b2d2bf1a 21
Soyoon 4:4324f20e4597 22 void Blue_isr(){ //inturupt
Soyoon 4:4324f20e4597 23 while(Blue.readable()){
Soyoon 4:4324f20e4597 24 Blue_buffer[1] = Blue_buffer[0];
Soyoon 4:4324f20e4597 25 Blue_buffer[0] = Blue.getc();
Soyoon 4:4324f20e4597 26 if (Blue_buffer[0] == '\n' && Blue_flag == 1){Blue_flag = 0; Blue_ok = 1; k=0;}
Soyoon 4:4324f20e4597 27 if (Blue_buffer[0] == '*'){Blue_flag=1;}
Soyoon 4:4324f20e4597 28 if (Blue_flag==1){Blue_msg[k] = Blue_buffer[0]; k++;}
Soyoon 1:cd11c1c592c7 29 }
Soyoon 4:4324f20e4597 30 }
Soyoon 0:6ac6b2d2bf1a 31
Soyoon 4:4324f20e4597 32 void get_Blue(float*input_roll, float*input_pitch, float*input_yaw, float*input_thr)
Soyoon 1:cd11c1c592c7 33 {
Soyoon 4:4324f20e4597 34 if (Blue_ok == 1){
Soyoon 4:4324f20e4597 35 Blue_ok = 0;
Soyoon 4:4324f20e4597 36 sscanf(Blue_msg, "*%f,%f,%f,%f\n", input_roll, input_pitch, input_yaw, input_thr);
Soyoon 1:cd11c1c592c7 37 }
Soyoon 4:4324f20e4597 38 }
Soyoon 1:cd11c1c592c7 39
Soyoon 3:e3e965924dde 40 void blue_trig_isr(){
Soyoon 4:4324f20e4597 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:4324f20e4597 47 if(in_data<0) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '-';}
Soyoon 4:4324f20e4597 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:4324f20e4597 50 if(cnt_num == under_point) {while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '.'; }
Soyoon 4:4324f20e4597 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:4324f20e4597 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:4324f20e4597 64 float alt_buffer[2], w_alt=0; // weight for LPF
Soyoon 2:9d0f979369cf 65
Soyoon 4:4324f20e4597 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:4324f20e4597 73 if(abs(alt_buffer[0]- alt_buffer[1])>20){
Soyoon 4:4324f20e4597 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:4324f20e4597 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:4324f20e4597 95 // AHRS 5V p27(RX) // 20Hz
Soyoon 1:cd11c1c592c7 96 ///////////////////////////////////////
Soyoon 4:4324f20e4597 97 Serial AHRS(p28, p27);
Soyoon 4:4324f20e4597 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:4324f20e4597 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:4324f20e4597 117 sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, velx, vely, velz);
Soyoon 4:4324f20e4597 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:4324f20e4597 124 // Servo 5V PWM // needed to check pin#
Soyoon 1:cd11c1c592c7 125 ///////////////////////////////////
Soyoon 4:4324f20e4597 126 Servo Throttle(p26);
Soyoon 4:4324f20e4597 127 Servo CS1(p25); //below
Soyoon 4:4324f20e4597 128 Servo CS2(p23); //upper
Soyoon 4:4324f20e4597 129 Servo CS3(p21); //below
Soyoon 4:4324f20e4597 130 Servo CS4(p22); //upper
Soyoon 4:4324f20e4597 131
Soyoon 4:4324f20e4597 132 float thr_value = 0.0, ctrl1_value = 0.5, ctrl2_value = 0.5, ctrl3_value = 0.5, ctrl4_value = 0.5;
Soyoon 4:4324f20e4597 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:4324f20e4597 134 float ctrl_roll = 0.0, ctrl_pitch = 0.0, ctrl_yaw = 0.0;
Soyoon 4:4324f20e4597 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:4324f20e4597 137 void ctrl_attitude(){
Soyoon 4:4324f20e4597 138 err_roll = input_roll - roll;
Soyoon 4:4324f20e4597 139 err_pitch = input_pitch - pitch;
Soyoon 4:4324f20e4597 140 err_yaw = input_yaw - yaw;
Soyoon 4:4324f20e4597 141 ctrl_roll = (kp1*err_roll)+(kd1*(err_roll-preerror_roll)/dt);
Soyoon 4:4324f20e4597 142 ctrl_pitch = (kp2*err_pitch)+(kd2*(err_pitch-preerror_pitch)/dt);
Soyoon 4:4324f20e4597 143 ctrl_yaw = (kp3*err_yaw)+(kd3*(err_yaw-preerror_yaw)/dt);
Soyoon 4:4324f20e4597 144 ctrl_roll = (ctrl_roll/180.0); // Range adjustment
Soyoon 4:4324f20e4597 145 ctrl_pitch = (ctrl_pitch/180.0);
Soyoon 4:4324f20e4597 146 ctrl_yaw = (ctrl_yaw/180.0);
Soyoon 4:4324f20e4597 147 preerror_roll = err_roll;
Soyoon 4:4324f20e4597 148 preerror_pitch = err_pitch;
Soyoon 4:4324f20e4597 149 preerror_yaw = err_yaw;
Soyoon 4:4324f20e4597 150 // Roll = ctrl2 - ctrl4
Soyoon 4:4324f20e4597 151 // Pitch = ctrl1 - ctrl3
Soyoon 4:4324f20e4597 152 // Yaw = ctrl1 + ctrl2 + ctrl3 + ctrl4
Soyoon 4:4324f20e4597 153 ctrl1_value = ((ctrl_pitch/2.0) + (ctrl_yaw/4.0)) + 0.5;
Soyoon 4:4324f20e4597 154 ctrl2_value = ((ctrl_roll/2.0) + (ctrl_yaw/4.0)) + 0.5;
Soyoon 4:4324f20e4597 155 ctrl3_value = (-(ctrl_pitch/2.0) + (ctrl_yaw/4.0)) + 0.5;
Soyoon 4:4324f20e4597 156 ctrl4_value = (-(ctrl_roll/2.0) + (ctrl_yaw/4.0)) + 0.5;
Soyoon 4:4324f20e4597 157 if (ctrl1_value<=0.0){ctrl1_value=0.0;}
Soyoon 4:4324f20e4597 158 else if (ctrl1_value>=1.0) {ctrl1_value=1.0;}
Soyoon 4:4324f20e4597 159 if (ctrl2_value<=0.0){ctrl2_value=0.0;}
Soyoon 4:4324f20e4597 160 else if (ctrl2_value>=1.0) {ctrl2_value=1.0;}
Soyoon 4:4324f20e4597 161 if (ctrl3_value<=0.0){ctrl3_value=0.0;}
Soyoon 4:4324f20e4597 162 else if (ctrl3_value>=1.0) {ctrl3_value=1.0;}
Soyoon 4:4324f20e4597 163 if (ctrl4_value<=0.0){ctrl4_value=0.0;}
Soyoon 4:4324f20e4597 164 else if (ctrl4_value>=1.0) {ctrl4_value=1.0;}
Soyoon 4:4324f20e4597 165 CS1 = ctrl1_value;
Soyoon 4:4324f20e4597 166 CS2 = ctrl2_value;
Soyoon 4:4324f20e4597 167 CS3 = ctrl3_value;
Soyoon 4:4324f20e4597 168 CS4 = ctrl4_value;
Soyoon 4:4324f20e4597 169 }
Soyoon 2:9d0f979369cf 170
Soyoon 4:4324f20e4597 171
Soyoon 4:4324f20e4597 172 void neutral(){
Soyoon 4:4324f20e4597 173 ctrl1_value = 0.5;
Soyoon 4:4324f20e4597 174 ctrl2_value = 0.5;
Soyoon 4:4324f20e4597 175 ctrl3_value = 0.5;
Soyoon 4:4324f20e4597 176 ctrl4_value = 0.5;
Soyoon 4:4324f20e4597 177 CS1 = ctrl1_value;
Soyoon 4:4324f20e4597 178 CS2 = ctrl2_value;
Soyoon 4:4324f20e4597 179 CS3 = ctrl3_value;
Soyoon 4:4324f20e4597 180 CS4 = ctrl4_value;
Soyoon 2:9d0f979369cf 181 }
Soyoon 1:cd11c1c592c7 182
Soyoon 1:cd11c1c592c7 183 ///////////////////////////////
Soyoon 1:cd11c1c592c7 184 // Datalogger Mbed //
Soyoon 1:cd11c1c592c7 185 ///////////////////////////////
Soyoon 1:cd11c1c592c7 186 LocalFileSystem local("local");
Soyoon 2:9d0f979369cf 187 int file_no=1;
Soyoon 2:9d0f979369cf 188 char filename[256];
Soyoon 1:cd11c1c592c7 189 FILE *fp;
Soyoon 1:cd11c1c592c7 190
Soyoon 2:9d0f979369cf 191 void Log_file(){
Soyoon 2:9d0f979369cf 192 sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
Soyoon 2:9d0f979369cf 193 fp = fopen(filename, "r"); // if file can be loaded, return is 1
Soyoon 2:9d0f979369cf 194 while(fp){
Soyoon 2:9d0f979369cf 195 fclose(fp);
Soyoon 2:9d0f979369cf 196 file_no ++;
Soyoon 2:9d0f979369cf 197 sprintf(filename, "/local/Data%d.txt", file_no); // Open "tem%d.txt" on the local file system for writing
Soyoon 2:9d0f979369cf 198 fp = fopen(filename, "r");
Soyoon 2:9d0f979369cf 199 }
Soyoon 2:9d0f979369cf 200 fp = fopen(filename, "w");
Soyoon 2:9d0f979369cf 201 }
Soyoon 2:9d0f979369cf 202
Soyoon 1:cd11c1c592c7 203 void Log_data(){
Soyoon 4:4324f20e4597 204 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 205 }
Soyoon 1:cd11c1c592c7 206
Soyoon 2:9d0f979369cf 207 void send_Blue(){
Soyoon 4:4324f20e4597 208 if (send_ok == 1){
Soyoon 4:4324f20e4597 209 send_ok = 0;
Soyoon 4:4324f20e4597 210 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '*';
Soyoon 3:e3e965924dde 211 trans_blue_data(roll,3,2);
Soyoon 4:4324f20e4597 212 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 3:e3e965924dde 213 trans_blue_data(pitch,3,2);
Soyoon 4:4324f20e4597 214 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 3:e3e965924dde 215 trans_blue_data(yaw,3,2);
Soyoon 4:4324f20e4597 216 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 4:4324f20e4597 217 trans_blue_data(alt,2,2);
Soyoon 4:4324f20e4597 218 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 4:4324f20e4597 219 trans_blue_data(velxyz,2,2);
Soyoon 4:4324f20e4597 220 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = ',';
Soyoon 4:4324f20e4597 221 trans_blue_data(velz,2,2);
Soyoon 4:4324f20e4597 222 while((LPC_UART1->LSR&0x20)==0); LPC_UART1->THR = '\n';
Soyoon 3:e3e965924dde 223 }
Soyoon 2:9d0f979369cf 224 }
Soyoon 2:9d0f979369cf 225
Soyoon 3:e3e965924dde 226
Soyoon 2:9d0f979369cf 227 /////////////////////////////////
Soyoon 3:e3e965924dde 228 // Main loop //
Soyoon 2:9d0f979369cf 229 /////////////////////////////////
Soyoon 2:9d0f979369cf 230
Soyoon 0:6ac6b2d2bf1a 231 int main(void)
Soyoon 1:cd11c1c592c7 232 {
Soyoon 1:cd11c1c592c7 233 AHRS.baud(9600);
Soyoon 4:4324f20e4597 234 Blue.baud(9600);
Soyoon 4:4324f20e4597 235 Blue.attach(&Blue_isr);
Soyoon 3:e3e965924dde 236 AHRS.attach(&AHRS_isr);
Soyoon 4:4324f20e4597 237 blue_trig.attach(&blue_trig_isr, 0.1);
Soyoon 4:4324f20e4597 238 neutral();
Soyoon 4:4324f20e4597 239 Log_file();
Soyoon 1:cd11c1c592c7 240 while(1) {
Soyoon 4:4324f20e4597 241 get_AHRS(&roll,&pitch,&yaw,&velx,&vely,&velz,&velxyz);
Soyoon 4:4324f20e4597 242 get_Blue(&input_roll,&input_pitch,&input_yaw,&input_thr);
Soyoon 4:4324f20e4597 243 get_Baro(&alt);
Soyoon 4:4324f20e4597 244 if (input_roll==0){input_roll = roll;}
Soyoon 4:4324f20e4597 245 if (input_pitch==0){input_pitch = pitch;}
Soyoon 4:4324f20e4597 246 if (input_yaw==0){input_yaw = yaw;}
Soyoon 4:4324f20e4597 247 ctrl_attitude();
Soyoon 2:9d0f979369cf 248 Log_data();
Soyoon 2:9d0f979369cf 249 send_Blue();
Soyoon 4:4324f20e4597 250 if (input_roll==180.0 && input_pitch==180.0 && input_yaw==180.0 && input_thr==0){fclose(fp); break;}
Soyoon 1:cd11c1c592c7 251 }
Soyoon 1:cd11c1c592c7 252 }