Manuel Losada / Mbed OS ArduinoCom

this is befre i made a major change in program

main.cpp@2:c463326536ae, 2019-11-26 (annotated)

Committer:
mlosa010
Date:
Tue Nov 26 20:55:32 2019 +0000
Revision:
2:c463326536ae
Parent:
1:a8a5c74bdfa6 
this is before i added threading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mlosa010 0:4ff212e163fe 1 /* mbed Microcontroller Library
mlosa010 0:4ff212e163fe 2 * Copyright (c) 2019 ARM Limited
mlosa010 0:4ff212e163fe 3 * SPDX-License-Identifier: Apache-2.0
mlosa010 0:4ff212e163fe 4 */
mlosa010 0:4ff212e163fe 5
mlosa010 0:4ff212e163fe 6 #include "mbed.h"
mlosa010 0:4ff212e163fe 7 #include "math.h"
mlosa010 0:4ff212e163fe 8 #include "platform/mbed_thread.h"
mlosa010 0:4ff212e163fe 9
mlosa010 0:4ff212e163fe 10
mlosa010 0:4ff212e163fe 11 // Blinking rate in milliseconds
mlosa010 0:4ff212e163fe 12 #define BLINKING_RATE_MS 500
mlosa010 2:c463326536ae 13 Thread thre;
mlosa010 0:4ff212e163fe 14 Serial pc(USBTX, USBRX);
mlosa010 0:4ff212e163fe 15 Serial Ardu(p9,p10);
mlosa010 1:a8a5c74bdfa6 16 void inline MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX);
mlosa010 1:a8a5c74bdfa6 17 float inline filteredAngle(float *pitch, float *gyrX);
mlosa010 0:4ff212e163fe 18 int main()
mlosa010 0:4ff212e163fe 19 {
mlosa010 0:4ff212e163fe 20 uint8_t charArr[6];
mlosa010 1:a8a5c74bdfa6 21 int16_t accy;
mlosa010 1:a8a5c74bdfa6 22 int16_t accz;
mlosa010 1:a8a5c74bdfa6 23 int16_t gyr;
mlosa010 0:4ff212e163fe 24 float angleX =0;
mlosa010 0:4ff212e163fe 25 float samplerate = 9600.0;
mlosa010 0:4ff212e163fe 26 float finalAx=0;
mlosa010 0:4ff212e163fe 27 float pitch =0;
mlosa010 0:4ff212e163fe 28 float offset =0;
mlosa010 0:4ff212e163fe 29 float gyr_with_ofset =0;
mlosa010 0:4ff212e163fe 30 int k =1; // this is for getting the initial angle the first time around so we can start at zero
mlosa010 0:4ff212e163fe 31 int j=0;
mlosa010 0:4ff212e163fe 32 while (true) {
mlosa010 1:a8a5c74bdfa6 33 MPU6050Values(&Ardu,&accy,&accz,&gyr, &samplerate, &k, &offset,&pitch, &angleX);
mlosa010 1:a8a5c74bdfa6 34 finalAx = filteredAngle(&pitch, &angleX);
mlosa010 1:a8a5c74bdfa6 35
mlosa010 1:a8a5c74bdfa6 36
mlosa010 1:a8a5c74bdfa6 37
mlosa010 1:a8a5c74bdfa6 38
mlosa010 1:a8a5c74bdfa6 39
mlosa010 1:a8a5c74bdfa6 40
mlosa010 1:a8a5c74bdfa6 41
mlosa010 1:a8a5c74bdfa6 42
mlosa010 1:a8a5c74bdfa6 43
mlosa010 1:a8a5c74bdfa6 44
mlosa010 1:a8a5c74bdfa6 45 if(j=1000){
mlosa010 1:a8a5c74bdfa6 46 pc.printf("calc angle: %f.2\n\r",finalAx);
mlosa010 1:a8a5c74bdfa6 47 j=0;
mlosa010 1:a8a5c74bdfa6 48 }else
mlosa010 1:a8a5c74bdfa6 49 j++;
mlosa010 1:a8a5c74bdfa6 50 /*
mlosa010 0:4ff212e163fe 51 for(int i =0; i<6;i++)
mlosa010 0:4ff212e163fe 52 charArr[i]=Ardu.getc();
mlosa010 0:4ff212e163fe 53
mlosa010 0:4ff212e163fe 54 int16_t accy = charArr[0]<<8|charArr[1];
mlosa010 0:4ff212e163fe 55 int16_t accz = charArr[2]<<8|charArr[3];
mlosa010 0:4ff212e163fe 56 int16_t gyr = charArr[4]<<8|charArr[5];
mlosa010 0:4ff212e163fe 57 //pc.printf(" Acc: %d Gyr: %d\n\r",acc, gyr);
mlosa010 0:4ff212e163fe 58 pitch = atan(accy/accz);// angle to check the gyro data against in the complementary filter
mlosa010 0:4ff212e163fe 59 if(k){
mlosa010 0:4ff212e163fe 60 offset = gyr/samplerate;//gets the offset or starting point the first time around
mlosa010 0:4ff212e163fe 61 k=0;
mlosa010 0:4ff212e163fe 62 }
mlosa010 0:4ff212e163fe 63
mlosa010 0:4ff212e163fe 64 gyr_with_ofset = gyr - offset;
mlosa010 0:4ff212e163fe 65 angleX += gyr_with_ofset/samplerate;//intergration for the angular velocity
mlosa010 0:4ff212e163fe 66 finalAx = pitch*(0.02)+angleX*(0.98);//complementary filter
mlosa010 0:4ff212e163fe 67 //the following is just so the calculated angle prints at a reasonable rate
mlosa010 0:4ff212e163fe 68 if(j=1000){
mlosa010 0:4ff212e163fe 69 pc.printf("calc angle: %f.2\n\r",finalAx);
mlosa010 0:4ff212e163fe 70 j=0;
mlosa010 0:4ff212e163fe 71 }else
mlosa010 0:4ff212e163fe 72 j++;
mlosa010 1:a8a5c74bdfa6 73 */
mlosa010 0:4ff212e163fe 74 }
mlosa010 0:4ff212e163fe 75 }
mlosa010 1:a8a5c74bdfa6 76
mlosa010 1:a8a5c74bdfa6 77 inline void MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX){
mlosa010 1:a8a5c74bdfa6 78 uint8_t charArr[6];
mlosa010 1:a8a5c74bdfa6 79 for(int i =0; i<6;i++)
mlosa010 1:a8a5c74bdfa6 80 charArr[i]=Ardu->getc();
mlosa010 1:a8a5c74bdfa6 81
mlosa010 1:a8a5c74bdfa6 82 (*accy)= charArr[0]<<8|charArr[1];
mlosa010 1:a8a5c74bdfa6 83 (*accz)= charArr[2]<<8|charArr[3];
mlosa010 1:a8a5c74bdfa6 84 (*gyr)= charArr[4]<<8|charArr[5];
mlosa010 1:a8a5c74bdfa6 85 (*pitch) = atan((*accy)/(*accz));
mlosa010 1:a8a5c74bdfa6 86 if(*k){
mlosa010 1:a8a5c74bdfa6 87 (*offset) = (*gyr);//gets the offset or starting point the first time around
mlosa010 1:a8a5c74bdfa6 88 *k=0;
mlosa010 1:a8a5c74bdfa6 89 }
mlosa010 1:a8a5c74bdfa6 90
mlosa010 1:a8a5c74bdfa6 91 (*angleX) += ((*gyr) - (*offset))/(*samplerate);
mlosa010 1:a8a5c74bdfa6 92 }
mlosa010 1:a8a5c74bdfa6 93
mlosa010 1:a8a5c74bdfa6 94 inline float filteredAngle(float *pitch, float *angleX){
mlosa010 1:a8a5c74bdfa6 95 return (*pitch)*(0.02)+(*angleX)*(0.98);
mlosa010 1:a8a5c74bdfa6 96 }