CANSAT_AIRFUL / Mbed 2 deprecated mbed_paper_mixing

mixing control

Dependencies:   Servo mbed

Fork of mbed_main by CANSAT_AIRFUL

main.cpp@0:6ac6b2d2bf1a, 2016-07-08 (annotated)

Committer:
Soyoon
Date:
Fri Jul 08 05:39:27 2016 +0000
Revision:
0:6ac6b2d2bf1a
Child:
1:cd11c1c592c7
test

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 0:6ac6b2d2bf1a 3 #include "Ultrasonic.h"
Soyoon 0:6ac6b2d2bf1a 4 #include "math.h"
Soyoon 0:6ac6b2d2bf1a 5 Serial pc(USBTX, USBRX);
Soyoon 0:6ac6b2d2bf1a 6 Barometer barometer(p9, p10);
Soyoon 0:6ac6b2d2bf1a 7 Ultrasonic sonar(p24, p12);
Soyoon 0:6ac6b2d2bf1a 8 Serial AHRS(p13, p14);
Soyoon 0:6ac6b2d2bf1a 9 Serial GPS(p28, p27);
Soyoon 0:6ac6b2d2bf1a 10 PwmOut Linear(p21);
Soyoon 0:6ac6b2d2bf1a 11 PwmOut Micro_gf(p22);
Soyoon 0:6ac6b2d2bf1a 12 PwmOut Micro_d(p23);
Soyoon 0:6ac6b2d2bf1a 13
Soyoon 0:6ac6b2d2bf1a 14 float roll,pitch,yaw,accx,accy,accz;
Soyoon 0:6ac6b2d2bf1a 15 float p = 0.0f, t = 0.0f, alt = 0.0f;
Soyoon 0:6ac6b2d2bf1a 16 int i = 0, j=0, gps_ok=0, flag, lock
Soyoon 0:6ac6b2d2bf1a 17 int stat, chksum=0;
Soyoon 0:6ac6b2d2bf1a 18 volatile unsigned char GPS_buffer[2];
Soyoon 0:6ac6b2d2bf1a 19 char ns, ew;
Soyoon 0:6ac6b2d2bf1a 20 char msg[150];
Soyoon 0:6ac6b2d2bf1a 21
Soyoon 0:6ac6b2d2bf1a 22 void GPS_isr(){
Soyoon 0:6ac6b2d2bf1a 23 i++;
Soyoon 0:6ac6b2d2bf1a 24 GPS_buffer[1] = GPS_buffer[0];
Soyoon 0:6ac6b2d2bf1a 25 GPS_buffer[0] = GPS.getc();
Soyoon 0:6ac6b2d2bf1a 26 if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10))
Soyoon 0:6ac6b2d2bf1a 27 {
Soyoon 0:6ac6b2d2bf1a 28 i=0;
Soyoon 0:6ac6b2d2bf1a 29 if (flag == 1)
Soyoon 0:6ac6b2d2bf1a 30 {
Soyoon 0:6ac6b2d2bf1a 31 flag = 0;
Soyoon 0:6ac6b2d2bf1a 32 gps_ok = 1;
Soyoon 0:6ac6b2d2bf1a 33 j=0;
Soyoon 0:6ac6b2d2bf1a 34 }
Soyoon 0:6ac6b2d2bf1a 35 }
Soyoon 0:6ac6b2d2bf1a 36 if ((i==5)&(GPS_buffer[0] == 'G')){flag=1;}
Soyoon 0:6ac6b2d2bf1a 37 if (flag==1){msg[j]=GPS_buffer[0]; j++;}
Soyoon 0:6ac6b2d2bf1a 38 }
Soyoon 0:6ac6b2d2bf1a 39
Soyoon 0:6ac6b2d2bf1a 40 float trunc(float v) {
Soyoon 0:6ac6b2d2bf1a 41 if(v < 0.0) {
Soyoon 0:6ac6b2d2bf1a 42 v*= -1.0;
Soyoon 0:6ac6b2d2bf1a 43 v = floor(v);
Soyoon 0:6ac6b2d2bf1a 44 v*=-1.0;
Soyoon 0:6ac6b2d2bf1a 45 } else {v = floor(v);}
Soyoon 0:6ac6b2d2bf1a 46 return v;
Soyoon 0:6ac6b2d2bf1a 47 }
Soyoon 0:6ac6b2d2bf1a 48
Soyoon 0:6ac6b2d2bf1a 49 int main(void)
Soyoon 0:6ac6b2d2bf1a 50 { AHRS.baud(9600);
Soyoon 0:6ac6b2d2bf1a 51 GPS.baud(9600);
Soyoon 0:6ac6b2d2bf1a 52 GPS.attach(&GPS_isr, Serial::RxIrq);
Soyoon 0:6ac6b2d2bf1a 53 float longitude,latitude, time;
Soyoon 0:6ac6b2d2bf1a 54 Linear.period(0.01); // set PWM period to 1ms
Soyoon 0:6ac6b2d2bf1a 55 Micro_gf.period(0.01);
Soyoon 0:6ac6b2d2bf1a 56 Micro_d.period(0.01);
Soyoon 0:6ac6b2d2bf1a 57 //Linear=0.1;wait(1);
Soyoon 0:6ac6b2d2bf1a 58 Linear=0.19;
Soyoon 0:6ac6b2d2bf1a 59 Micro_d=0.2;
Soyoon 0:6ac6b2d2bf1a 60 Micro_gf=0.2;
Soyoon 0:6ac6b2d2bf1a 61 wait(1);
Soyoon 0:6ac6b2d2bf1a 62 while(1) {
Soyoon 0:6ac6b2d2bf1a 63 //stat should be added
Soyoon 0:6ac6b2d2bf1a 64 barometer.update();
Soyoon 0:6ac6b2d2bf1a 65 p = barometer.get_pressure();
Soyoon 0:6ac6b2d2bf1a 66 t = barometer.get_temperature();
Soyoon 0:6ac6b2d2bf1a 67 alt = barometer.get_altitude_m();
Soyoon 0:6ac6b2d2bf1a 68 while(AHRS.getc() != '\n');
Soyoon 0:6ac6b2d2bf1a 69 AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", &roll, &pitch, &yaw, &accx,&accy,&accz);
Soyoon 0:6ac6b2d2bf1a 70 if (gps_ok == 1)
Soyoon 0:6ac6b2d2bf1a 71 {gps_ok = 0;
Soyoon 0:6ac6b2d2bf1a 72 sscanf(msg, "GA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock);
Soyoon 0:6ac6b2d2bf1a 73 if(ns == 'S') {latitude *= -1.0; }
Soyoon 0:6ac6b2d2bf1a 74 if(ew == 'W') {longitude *= -1.0; }
Soyoon 0:6ac6b2d2bf1a 75 float degrees = trunc(latitude / 100.0f);
Soyoon 0:6ac6b2d2bf1a 76 float minutes = latitude - (degrees * 100.0f);
Soyoon 0:6ac6b2d2bf1a 77 latitude = degrees + minutes / 60.0f;
Soyoon 0:6ac6b2d2bf1a 78 degrees = trunc(longitude / 100.0f);
Soyoon 0:6ac6b2d2bf1a 79 minutes = longitude - (degrees * 100.0f);
Soyoon 0:6ac6b2d2bf1a 80 longitude = degrees + minutes / 60.0f;
Soyoon 0:6ac6b2d2bf1a 81 time = time + 90000;
Soyoon 0:6ac6b2d2bf1a 82 }
Soyoon 0:6ac6b2d2bf1a 83 unsigned int distance = sonar.distance();
Soyoon 0:6ac6b2d2bf1a 84 if (distance<10){Linear =0.1;}
Soyoon 0:6ac6b2d2bf1a 85 chksum=(int)stat+t+alt+roll+pitch+yaw+accz+time+latitude+longitude+distance;
Soyoon 0:6ac6b2d2bf1a 86 pc.printf("\r\n Temperature(c):%f Altitude(m):%f Roll:%f Pitch:%f Yaw:%f Accz:%f Time:%f Latitude:%f Longitude:%f Sonar(cm)%u",stat,t,alt,roll,pitch,yaw,accz,time,latitude,longitude,distance,chksum);
Soyoon 0:6ac6b2d2bf1a 87 }
Soyoon 0:6ac6b2d2bf1a 88 }
Soyoon 0:6ac6b2d2bf1a 89