Manuel Losada / Mbed 2 deprecated HCSR04andMPU6050

attempt to intergrate gyro and distance

Dependencies:   mbed TextLCD

main.cpp@8:a1d19a540c83, 2020-02-02 (annotated)

Committer:
jrive262
Date:
Sun Feb 02 21:18:44 2020 +0000
Revision:
8:a1d19a540c83
Parent:
7:164cf2903226 
Code as of 2/2/20

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mlosa010 0:c1b0d03af2ac 1 #include "mbed.h"
mlosa010 1:a58d5b79517e 2 #include "math.h"
mlosa010 1:a58d5b79517e 3 #include "TextLCD.h"
mlosa010 0:c1b0d03af2ac 4 #define LENOFBOT 2.5
mlosa010 0:c1b0d03af2ac 5 #define LENOFSENSOR 0.394
mlosa010 6:3b26e4ef0364 6 #define ERROR 0.5
jrive262 2:418fc2dabdb5 7 //obj for distance sensor
jrive262 2:418fc2dabdb5 8
mlosa010 0:c1b0d03af2ac 9 Serial pc(USBTX, USBRX);
mlosa010 6:3b26e4ef0364 10 DigitalOut solenoid(p11);
mlosa010 3:035bfd6f39b5 11 DigitalOut led1(LED1);
mlosa010 3:035bfd6f39b5 12 DigitalOut led2(LED2);
mlosa010 3:035bfd6f39b5 13 DigitalOut led3(LED3);
mlosa010 3:035bfd6f39b5 14 DigitalOut led4(LED4);
jrive262 8:a1d19a540c83 15 PwmOut Rwheel(p21);
jrive262 8:a1d19a540c83 16 PwmOut Lwheel(p22);
mlosa010 0:c1b0d03af2ac 17 PwmOut trigger(p24);
mlosa010 0:c1b0d03af2ac 18 PwmOut trigger_b(p23);
mlosa010 6:3b26e4ef0364 19 InterruptIn echoFront(p12,PullDown);
mlosa010 6:3b26e4ef0364 20 InterruptIn echoBack(p13,PullDown);
mlosa010 0:c1b0d03af2ac 21 Timer timer;
mlosa010 0:c1b0d03af2ac 22 Timer timer_b;
mlosa010 1:a58d5b79517e 23 //functions for distance sensor
mlosa010 0:c1b0d03af2ac 24 volatile float front_reading;
mlosa010 0:c1b0d03af2ac 25 volatile float back_sensor;
mlosa010 1:a58d5b79517e 26 void stop_back (void);
mlosa010 1:a58d5b79517e 27 void start_back(void);
mlosa010 1:a58d5b79517e 28 void stop (void);
mlosa010 1:a58d5b79517e 29 void start(void);
mlosa010 3:035bfd6f39b5 30 void forward(void);
mlosa010 3:035bfd6f39b5 31 void backward(void);
mlosa010 3:035bfd6f39b5 32 void rotateRight(void);
mlosa010 3:035bfd6f39b5 33 void rotateLeft(void);
mlosa010 4:47530a802496 34 void stopMotors(void);
mlosa010 1:a58d5b79517e 35 //functions for MPU
mlosa010 1:a58d5b79517e 36 Serial Ardu(p9,p10);
mlosa010 1:a58d5b79517e 37 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:a58d5b79517e 38 float inline filteredAngle(float *pitch, float *gyrX);
jrive262 2:418fc2dabdb5 39
mlosa010 1:a58d5b79517e 40 //LCD
jrive262 2:418fc2dabdb5 41 I2C i2c_lcd(p28,p27);
mlosa010 1:a58d5b79517e 42 TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD16x2);
jrive262 2:418fc2dabdb5 43
jrive262 2:418fc2dabdb5 44
mlosa010 1:a58d5b79517e 45 int main() {
mlosa010 0:c1b0d03af2ac 46 int16_t accy;
mlosa010 0:c1b0d03af2ac 47 int16_t accz;
mlosa010 0:c1b0d03af2ac 48 int16_t gyr;
mlosa010 0:c1b0d03af2ac 49 float angleX =0;
mlosa010 0:c1b0d03af2ac 50 float samplerate = 9600.0;
mlosa010 0:c1b0d03af2ac 51 float finalAx=0;
mlosa010 0:c1b0d03af2ac 52 float pitch =0;
mlosa010 0:c1b0d03af2ac 53 float offset =0;
mlosa010 6:3b26e4ef0364 54 float center_distance;
jrive262 2:418fc2dabdb5 55 // float gyr_with_ofset =0;
mlosa010 0:c1b0d03af2ac 56 int k =1; // this is for getting the initial angle the first time around so we can start at zero
jrive262 2:418fc2dabdb5 57 // int j=0;
jrive262 2:418fc2dabdb5 58
mlosa010 1:a58d5b79517e 59 //distance sensor stuff
mlosa010 6:3b26e4ef0364 60 echoFront.rise(&start);
mlosa010 6:3b26e4ef0364 61 echoFront.fall(&stop);
mlosa010 6:3b26e4ef0364 62 echoBack.rise(&start_back);
mlosa010 6:3b26e4ef0364 63 echoBack.fall(&stop_back);
mlosa010 1:a58d5b79517e 64 trigger.period(0.06);
mlosa010 4:47530a802496 65 trigger_b.period(0.06);
mlosa010 7:164cf2903226 66 int pi[]= {3,1,4,1,5,9,2,6,5,3,5,8,9,7,9,3,2,3,8,4,6,2,6,4,3,3,8,3,2,7,9,5,0,2,8,8,4,1,9,7,1,6,9,3,9,9,3,7,5,1,0,5,8,2,0,9,7,4,9,4,4,5,9,2,3,0,7,8,1,6,4,0,6,2,8,6,2,0,8,9,9,8,6,2,8,0,3,4,8,2,5,3,4,2,1,1,7,0,6,7,9};
mlosa010 1:a58d5b79517e 67 int currentdigit =0;
mlosa010 6:3b26e4ef0364 68 int target_front_distance;
mlosa010 1:a58d5b79517e 69 int back_distance;
mlosa010 3:035bfd6f39b5 70
mlosa010 3:035bfd6f39b5 71 //motors
mlosa010 3:035bfd6f39b5 72 Lwheel.period_us(21700);
mlosa010 3:035bfd6f39b5 73 Rwheel.period_us(21700);
mlosa010 1:a58d5b79517e 74 while(1) {
mlosa010 6:3b26e4ef0364 75
mlosa010 3:035bfd6f39b5 76 led1=0;
mlosa010 3:035bfd6f39b5 77 led2=0;
mlosa010 3:035bfd6f39b5 78 led3=0;
mlosa010 3:035bfd6f39b5 79 led4=0;
mlosa010 0:c1b0d03af2ac 80 trigger.pulsewidth_us(10);
mlosa010 3:035bfd6f39b5 81 trigger_b.pulsewidth_us(10);
mlosa010 6:3b26e4ef0364 82 target_front_distance = (3*pi[currentdigit])+9;
mlosa010 6:3b26e4ef0364 83 back_distance= 27-target_front_distance;
mlosa010 0:c1b0d03af2ac 84 MPU6050Values(&Ardu,&accy,&accz,&gyr, &samplerate, &k, &offset,&pitch, &angleX);
mlosa010 0:c1b0d03af2ac 85 finalAx = filteredAngle(&pitch, &angleX);
mlosa010 7:164cf2903226 86
mlosa010 7:164cf2903226 87 lcd.printf("A: %.2f\n\r",finalAx);
mlosa010 6:3b26e4ef0364 88 center_distance = (front_reading/2)+LENOFBOT+LENOFSENSOR;
mlosa010 7:164cf2903226 89 if(finalAx>+0.5){
mlosa010 7:164cf2903226 90 rotateLeft();
mlosa010 7:164cf2903226 91 }else if(finalAx<-0.5){
mlosa010 7:164cf2903226 92 rotateRight();
mlosa010 7:164cf2903226 93 }
mlosa010 6:3b26e4ef0364 94 if(center_distance<(target_front_distance+ERROR)){
mlosa010 6:3b26e4ef0364 95 pc.printf("moving back distance read: %.3f digit: %d\n\r",center_distance,target_front_distance);
mlosa010 3:035bfd6f39b5 96 backward();
mlosa010 5:9cbd4a5a4bda 97 //if(((back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance-0.1||(back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance+0.1))
mlosa010 5:9cbd4a5a4bda 98
mlosa010 4:47530a802496 99 }
mlosa010 5:9cbd4a5a4bda 100
mlosa010 6:3b26e4ef0364 101 if(center_distance>(target_front_distance-ERROR)){
mlosa010 6:3b26e4ef0364 102 pc.printf("moving forward distance read: %.3f should be %d\n\r",center_distance,target_front_distance);
mlosa010 3:035bfd6f39b5 103 forward();
mlosa010 5:9cbd4a5a4bda 104 //if((back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance-0.1||(back_sensor/2)+LENOFBOT+LENOFSENSOR <= back_distance+0.1)
mlosa010 5:9cbd4a5a4bda 105
mlosa010 0:c1b0d03af2ac 106 }
mlosa010 5:9cbd4a5a4bda 107
mlosa010 6:3b26e4ef0364 108 if(center_distance>(target_front_distance-ERROR)&&center_distance<(target_front_distance+ERROR)){
mlosa010 4:47530a802496 109 stopMotors();
jrive262 8:a1d19a540c83 110 pc.printf("stopped distance is: %.3f should be %d\n\r",center_distance,target_front_distance);
mlosa010 6:3b26e4ef0364 111 solenoid =1;
mlosa010 6:3b26e4ef0364 112 wait_ms(100);
mlosa010 6:3b26e4ef0364 113 solenoid =0;
mlosa010 7:164cf2903226 114 currentdigit++;
mlosa010 5:9cbd4a5a4bda 115 }
mlosa010 0:c1b0d03af2ac 116 }
mlosa010 0:c1b0d03af2ac 117 }
mlosa010 0:c1b0d03af2ac 118
mlosa010 0:c1b0d03af2ac 119 void start(void){
mlosa010 0:c1b0d03af2ac 120 timer.start();
mlosa010 3:035bfd6f39b5 121 led4=1;
mlosa010 3:035bfd6f39b5 122 led3=0;
mlosa010 0:c1b0d03af2ac 123 }
mlosa010 0:c1b0d03af2ac 124 void stop (void){
mlosa010 0:c1b0d03af2ac 125 timer.stop();
mlosa010 0:c1b0d03af2ac 126 front_reading = (float)timer.read()*13504;
mlosa010 0:c1b0d03af2ac 127 timer.reset();
mlosa010 3:035bfd6f39b5 128 led3=1;
mlosa010 3:035bfd6f39b5 129 led4=0;
mlosa010 0:c1b0d03af2ac 130 }
mlosa010 0:c1b0d03af2ac 131 void start_back(void){
mlosa010 0:c1b0d03af2ac 132 timer_b.start();
mlosa010 3:035bfd6f39b5 133 led2=1;
mlosa010 0:c1b0d03af2ac 134 }
mlosa010 0:c1b0d03af2ac 135 void stop_back (void){
mlosa010 0:c1b0d03af2ac 136 timer_b.stop();
mlosa010 0:c1b0d03af2ac 137 back_sensor = (float)timer_b.read()*13480;
mlosa010 0:c1b0d03af2ac 138 timer_b.reset();
mlosa010 3:035bfd6f39b5 139 led1=1;
mlosa010 1:a58d5b79517e 140 }
mlosa010 1:a58d5b79517e 141
mlosa010 1:a58d5b79517e 142
mlosa010 1:a58d5b79517e 143 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:a58d5b79517e 144 uint8_t charArr[6];
mlosa010 1:a58d5b79517e 145 for(int i =0; i<6;i++)
mlosa010 1:a58d5b79517e 146 charArr[i]=Ardu->getc();
mlosa010 1:a58d5b79517e 147
mlosa010 1:a58d5b79517e 148 (*accy)= charArr[0]<<8|charArr[1];
mlosa010 1:a58d5b79517e 149 (*accz)= charArr[2]<<8|charArr[3];
mlosa010 1:a58d5b79517e 150 (*gyr)= charArr[4]<<8|charArr[5];
mlosa010 1:a58d5b79517e 151 float thing=(*accy)/(*accz);
mlosa010 1:a58d5b79517e 152 (*pitch) = atan(thing);
mlosa010 1:a58d5b79517e 153 if(*k){
mlosa010 1:a58d5b79517e 154 (*offset) = (*gyr);//gets the offset or starting point the first time around
mlosa010 1:a58d5b79517e 155 *k=0;
mlosa010 1:a58d5b79517e 156 }
mlosa010 1:a58d5b79517e 157
mlosa010 1:a58d5b79517e 158 (*angleX) += ((*gyr) - (*offset))/(*samplerate);
mlosa010 1:a58d5b79517e 159 }
mlosa010 1:a58d5b79517e 160
mlosa010 1:a58d5b79517e 161 inline float filteredAngle(float *pitch, float *angleX){
mlosa010 7:164cf2903226 162 return ((*pitch)*(0.5)+(*angleX)*(0.95))+0.25;
mlosa010 3:035bfd6f39b5 163 }
mlosa010 3:035bfd6f39b5 164
mlosa010 3:035bfd6f39b5 165
mlosa010 6:3b26e4ef0364 166 void backward(void){
mlosa010 6:3b26e4ef0364 167 Lwheel.pulsewidth_us(1550);
mlosa010 6:3b26e4ef0364 168 Rwheel.pulsewidth_us(1450);
mlosa010 3:035bfd6f39b5 169 }
mlosa010 6:3b26e4ef0364 170 void forward(void){
mlosa010 6:3b26e4ef0364 171 Lwheel.pulsewidth_us(1450);
mlosa010 6:3b26e4ef0364 172 Rwheel.pulsewidth_us(1550);
mlosa010 3:035bfd6f39b5 173 }
mlosa010 3:035bfd6f39b5 174 void rotateRight(void){
mlosa010 3:035bfd6f39b5 175 Lwheel.pulsewidth_us(1700);
mlosa010 3:035bfd6f39b5 176 Rwheel.pulsewidth_us(1700);
mlosa010 3:035bfd6f39b5 177 }
mlosa010 3:035bfd6f39b5 178 void rotateLeft(void){
mlosa010 3:035bfd6f39b5 179 Lwheel.pulsewidth_us(1300);
mlosa010 3:035bfd6f39b5 180 Rwheel.pulsewidth_us(1300);
mlosa010 4:47530a802496 181 }
mlosa010 4:47530a802496 182 void stopMotors(void){
mlosa010 4:47530a802496 183 Lwheel.pulsewidth_us(1500);
mlosa010 4:47530a802496 184 Rwheel.pulsewidth_us(1500);
mlosa010 0:c1b0d03af2ac 185 }