WRCE TSD / Mbed 2 deprecated BNOser_servo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BNO055_fusion

main.cpp@0:6e9da949a2a3, 2019-09-18 (annotated)

Committer:
pmmccorkell
Date:
Wed Sep 18 11:14:10 2019 +0000
Revision:
0:6e9da949a2a3
last known code of land UAV

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmmccorkell 0:6e9da949a2a3 1 #include "mbed.h"
pmmccorkell 0:6e9da949a2a3 2 #include "BNO055.h"
pmmccorkell 0:6e9da949a2a3 3
pmmccorkell 0:6e9da949a2a3 4 Serial pc(USBTX, USBRX);
pmmccorkell 0:6e9da949a2a3 5 I2C i2c(p9,p10);
pmmccorkell 0:6e9da949a2a3 6 DigitalOut pwr_on(p30);
pmmccorkell 0:6e9da949a2a3 7 BNO055 imu(i2c,p26);
pmmccorkell 0:6e9da949a2a3 8 //DigitalOut led1(LED1);
pmmccorkell 0:6e9da949a2a3 9
pmmccorkell 0:6e9da949a2a3 10 //instantiate global ready to 0
pmmccorkell 0:6e9da949a2a3 11 uint32_t ready = 0x00000000;
pmmccorkell 0:6e9da949a2a3 12
pmmccorkell 0:6e9da949a2a3 13 BNO055_ID_INF_TypeDef bno055_id_inf;
pmmccorkell 0:6e9da949a2a3 14 BNO055_EULER_TypeDef euler_angles;
pmmccorkell 0:6e9da949a2a3 15 //BNO055_QUATERNION_TypeDef quaternion;
pmmccorkell 0:6e9da949a2a3 16 //BNO055_LIN_ACC_TypeDef linear_acc;
pmmccorkell 0:6e9da949a2a3 17 BNO055_GRAVITY_TypeDef gravity;
pmmccorkell 0:6e9da949a2a3 18 //BNO055_TEMPERATURE_TypeDef chip_temp;
pmmccorkell 0:6e9da949a2a3 19
pmmccorkell 0:6e9da949a2a3 20 // Instantiate motor driver PWM pins, duty cycle, and period
pmmccorkell 0:6e9da949a2a3 21 double stop_pw=.0015;
pmmccorkell 0:6e9da949a2a3 22 //double fwd_pw=.0018;
pmmccorkell 0:6e9da949a2a3 23 //double rev_pw=.0012;
pmmccorkell 0:6e9da949a2a3 24 PwmOut RH_motor(p23);
pmmccorkell 0:6e9da949a2a3 25 PwmOut Lfwd(p21); // left forward
pmmccorkell 0:6e9da949a2a3 26 PwmOut Lrev(p22); // left reverse
pmmccorkell 0:6e9da949a2a3 27 PwmOut Rfwd(p24); // right forward
pmmccorkell 0:6e9da949a2a3 28 PwmOut Rrev(p25); // right reverse
pmmccorkell 0:6e9da949a2a3 29 float dutycycle=0.40;
pmmccorkell 0:6e9da949a2a3 30
pmmccorkell 0:6e9da949a2a3 31
pmmccorkell 0:6e9da949a2a3 32 //Lazy... don't want to track these down everywhere
pmmccorkell 0:6e9da949a2a3 33 //Standard servo rate -> 50Hz
pmmccorkell 0:6e9da949a2a3 34 double servo_freq=50;
pmmccorkell 0:6e9da949a2a3 35 double servo_period=(1/servo_freq);
pmmccorkell 0:6e9da949a2a3 36 //25us per data sheet Blue Robotics BESC-R1
pmmccorkell 0:6e9da949a2a3 37 double esc_step=(25/1000000);
pmmccorkell 0:6e9da949a2a3 38 //400Hz update frequency per data sheet
pmmccorkell 0:6e9da949a2a3 39 int esc_freq=400;
pmmccorkell 0:6e9da949a2a3 40 double esc_time_ms=(1000/esc_freq);
pmmccorkell 0:6e9da949a2a3 41 //1.1-1.9ms pw per data sheet
pmmccorkell 0:6e9da949a2a3 42 double esc_range=.0004;
pmmccorkell 0:6e9da949a2a3 43 //natural #
pmmccorkell 0:6e9da949a2a3 44 double e=2.71828;
pmmccorkell 0:6e9da949a2a3 45
pmmccorkell 0:6e9da949a2a3 46
pmmccorkell 0:6e9da949a2a3 47
pmmccorkell 0:6e9da949a2a3 48
pmmccorkell 0:6e9da949a2a3 49 //Data function pulls data from BNO and sends over serial
pmmccorkell 0:6e9da949a2a3 50 //Accepts unsigned 32bit key word (k) that RasPi will use as trigger
pmmccorkell 0:6e9da949a2a3 51 uint16_t data(uint32_t k) {
pmmccorkell 0:6e9da949a2a3 52 //Instantiate status array of 6 32-bit words.
pmmccorkell 0:6e9da949a2a3 53 //First 16 bits of each 32-bit word are Identifiers for RasPi to correctly assign the trailing 16 bits of data.
pmmccorkell 0:6e9da949a2a3 54 uint32_t status[6]={0};
pmmccorkell 0:6e9da949a2a3 55
pmmccorkell 0:6e9da949a2a3 56 //word 0: Key
pmmccorkell 0:6e9da949a2a3 57 //Used to ensure Pi and Mbed are on same page.
pmmccorkell 0:6e9da949a2a3 58 status[0]=k;
pmmccorkell 0:6e9da949a2a3 59
pmmccorkell 0:6e9da949a2a3 60 //word 1: Status information.
pmmccorkell 0:6e9da949a2a3 61 //0xffff acts as prefix to identify Status for RasPi.
pmmccorkell 0:6e9da949a2a3 62 //Last 3 bits (from right) are current position (POS[0-7]). See BNO datasheet.
pmmccorkell 0:6e9da949a2a3 63 //4th bit (from right) is RH turn motors enabled.
pmmccorkell 0:6e9da949a2a3 64 //5th bit (from right) is LH turn motors enabled.
pmmccorkell 0:6e9da949a2a3 65 status[1]=ready;
pmmccorkell 0:6e9da949a2a3 66
pmmccorkell 0:6e9da949a2a3 67 //word 2: Calibration.
pmmccorkell 0:6e9da949a2a3 68 //0xc000 acts as prefix to identify Cal for RasPi.
pmmccorkell 0:6e9da949a2a3 69 status[2]=0xc0000000+imu.read_calib_status();
pmmccorkell 0:6e9da949a2a3 70
pmmccorkell 0:6e9da949a2a3 71 //Get Euler data from BNO.
pmmccorkell 0:6e9da949a2a3 72 imu.get_Euler_Angles(&euler_angles);
pmmccorkell 0:6e9da949a2a3 73
pmmccorkell 0:6e9da949a2a3 74 //word 3 is Heading.
pmmccorkell 0:6e9da949a2a3 75 //0xc100 acts as prefix to identify Heading for RasPi.
pmmccorkell 0:6e9da949a2a3 76 uint16_t h = euler_angles.h;
pmmccorkell 0:6e9da949a2a3 77 status[3]=0xc1000000+h;
pmmccorkell 0:6e9da949a2a3 78
pmmccorkell 0:6e9da949a2a3 79 //Offset calculation: 360*16bit resolution = 5760 -> converted to hex = 0x1680
pmmccorkell 0:6e9da949a2a3 80 int offset=0x1680;
pmmccorkell 0:6e9da949a2a3 81
pmmccorkell 0:6e9da949a2a3 82 //word 4 is Roll.
pmmccorkell 0:6e9da949a2a3 83 //0xc300 acts as prefix to identify Roll for RasPi.
pmmccorkell 0:6e9da949a2a3 84 //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
pmmccorkell 0:6e9da949a2a3 85 uint16_t r = offset + euler_angles.r;
pmmccorkell 0:6e9da949a2a3 86 status[4]=0xc3000000+r;
pmmccorkell 0:6e9da949a2a3 87
pmmccorkell 0:6e9da949a2a3 88 //word 5 is Pitch.
pmmccorkell 0:6e9da949a2a3 89 //0xc500 acts as prefix to identify Pitch for RasPi.
pmmccorkell 0:6e9da949a2a3 90 //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
pmmccorkell 0:6e9da949a2a3 91 uint16_t p = offset + euler_angles.p;
pmmccorkell 0:6e9da949a2a3 92 status[5]=0xc5000000+p;
pmmccorkell 0:6e9da949a2a3 93
pmmccorkell 0:6e9da949a2a3 94 //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi.
pmmccorkell 0:6e9da949a2a3 95 int i;
pmmccorkell 0:6e9da949a2a3 96 int l=(sizeof(status)/sizeof(uint32_t))-1;
pmmccorkell 0:6e9da949a2a3 97 for (i=0; i<=l; i++) {
pmmccorkell 0:6e9da949a2a3 98 pc.printf("%x\n", status[i]);
pmmccorkell 0:6e9da949a2a3 99 }
pmmccorkell 0:6e9da949a2a3 100
pmmccorkell 0:6e9da949a2a3 101 //return heading
pmmccorkell 0:6e9da949a2a3 102 return h;
pmmccorkell 0:6e9da949a2a3 103 }
pmmccorkell 0:6e9da949a2a3 104
pmmccorkell 0:6e9da949a2a3 105 //Function to setup motor PWM for right turn
pmmccorkell 0:6e9da949a2a3 106 void TurnRight() {
pmmccorkell 0:6e9da949a2a3 107 //mask off other bits and set Rmotor status to on
pmmccorkell 0:6e9da949a2a3 108 ready=(ready & 0xfffffff7) + 0x8;
pmmccorkell 0:6e9da949a2a3 109 // pc.printf("Turning Right\r\n");
pmmccorkell 0:6e9da949a2a3 110 Rfwd.write(dutycycle);
pmmccorkell 0:6e9da949a2a3 111 Lrev.write(dutycycle);
pmmccorkell 0:6e9da949a2a3 112 }
pmmccorkell 0:6e9da949a2a3 113
pmmccorkell 0:6e9da949a2a3 114 //Function to setup motor PWM for left turn
pmmccorkell 0:6e9da949a2a3 115 void TurnLeft() {
pmmccorkell 0:6e9da949a2a3 116 //mask off other bits and set Lmotor status to on
pmmccorkell 0:6e9da949a2a3 117 ready=(ready & 0xffffffef) + 0x10;
pmmccorkell 0:6e9da949a2a3 118 // pc.printf("Turning Left\r\n");
pmmccorkell 0:6e9da949a2a3 119 Lfwd.write(dutycycle);
pmmccorkell 0:6e9da949a2a3 120 Rrev.write(dutycycle);
pmmccorkell 0:6e9da949a2a3 121 }
pmmccorkell 0:6e9da949a2a3 122
pmmccorkell 0:6e9da949a2a3 123 //Function to stop all motors
pmmccorkell 0:6e9da949a2a3 124 void Stop_motors() {
pmmccorkell 0:6e9da949a2a3 125 //Set PW on all PWM pins to 0
pmmccorkell 0:6e9da949a2a3 126 Lfwd.write(0);
pmmccorkell 0:6e9da949a2a3 127 Lrev.write(0);
pmmccorkell 0:6e9da949a2a3 128 Rfwd.write(0);
pmmccorkell 0:6e9da949a2a3 129 Rrev.write(0);
pmmccorkell 0:6e9da949a2a3 130 //zero out Lm and Rm status bits
pmmccorkell 0:6e9da949a2a3 131 ready=(ready & 0xffffffe7);
pmmccorkell 0:6e9da949a2a3 132 }
pmmccorkell 0:6e9da949a2a3 133
pmmccorkell 0:6e9da949a2a3 134 //The sigmoid curve for logistic function.
pmmccorkell 0:6e9da949a2a3 135 //Used for continuous PWM and softstart / softstop near end points.
pmmccorkell 0:6e9da949a2a3 136 double Sigmoid(double Ymax, double m, double x0, double x) {
pmmccorkell 0:6e9da949a2a3 137 // double Logistic_func=(Ymax/(1+(e^((-1*m)*(x-x0)))));
pmmccorkell 0:6e9da949a2a3 138 double exponent=((-1*m)*(x-x0));
pmmccorkell 0:6e9da949a2a3 139 double power=pow(e,exponent);
pmmccorkell 0:6e9da949a2a3 140 double Logistic_func=Ymax/(1+power);
pmmccorkell 0:6e9da949a2a3 141 // pc.printf("exp: %f, power:%f, ret: %f\r\n",exponent,power,Logistic_func);
pmmccorkell 0:6e9da949a2a3 142 return Logistic_func;
pmmccorkell 0:6e9da949a2a3 143 }
pmmccorkell 0:6e9da949a2a3 144
pmmccorkell 0:6e9da949a2a3 145 //Function to control RH motor
pmmccorkell 0:6e9da949a2a3 146 void RH_motor_cntrl(double target_pw) {
pmmccorkell 0:6e9da949a2a3 147 double step_pw=.00005;
pmmccorkell 0:6e9da949a2a3 148 double current_pw=(RH_motor.read()*.02);
pmmccorkell 0:6e9da949a2a3 149 double diff_pw=abs(target_pw-current_pw);
pmmccorkell 0:6e9da949a2a3 150 //Time we want to take to get from 1.5 to [1.9 or 1.1]
pmmccorkell 0:6e9da949a2a3 151 // double Delay=0.5; //500ms
pmmccorkell 0:6e9da949a2a3 152 //Total # of steps / resolution of ESC
pmmccorkell 0:6e9da949a2a3 153 // double Steps=((.0019-.0015)/esc_step);
pmmccorkell 0:6e9da949a2a3 154 //Center of sigmoid curve
pmmccorkell 0:6e9da949a2a3 155 double Center=.1; //100ms -> ramp up within 200ms
pmmccorkell 0:6e9da949a2a3 156 //slope at Center
pmmccorkell 0:6e9da949a2a3 157 double Slope=50; //rate of ramp up
pmmccorkell 0:6e9da949a2a3 158 double time_count=0; //total time in while loop
pmmccorkell 0:6e9da949a2a3 159 double dwell_time=0.001; //time to wait between while loop
pmmccorkell 0:6e9da949a2a3 160 double tolerance_pw=.000045; //45us
pmmccorkell 0:6e9da949a2a3 161 double dir=0;
pmmccorkell 0:6e9da949a2a3 162 while(diff_pw>tolerance_pw) {
pmmccorkell 0:6e9da949a2a3 163 current_pw=(RH_motor.read()*.02);
pmmccorkell 0:6e9da949a2a3 164 diff_pw=abs(target_pw-current_pw);
pmmccorkell 0:6e9da949a2a3 165 if ((target_pw-current_pw)>=0) {
pmmccorkell 0:6e9da949a2a3 166 dir=(1);
pmmccorkell 0:6e9da949a2a3 167 //RH moving fwd
pmmccorkell 0:6e9da949a2a3 168 // ready=(ready & 0xfffffff7) + 0x8;
pmmccorkell 0:6e9da949a2a3 169 }
pmmccorkell 0:6e9da949a2a3 170 if ((target_pw-current_pw)<0) {
pmmccorkell 0:6e9da949a2a3 171 dir=(-1);
pmmccorkell 0:6e9da949a2a3 172 //RH moving rev
pmmccorkell 0:6e9da949a2a3 173 // ready=(ready & 0xffffffef) + 0x10;
pmmccorkell 0:6e9da949a2a3 174 }
pmmccorkell 0:6e9da949a2a3 175 double factor=Sigmoid(1,Slope,Center,time_count);
pmmccorkell 0:6e9da949a2a3 176 //double new_pw=stop_pw+(diff_pw*factor);
pmmccorkell 0:6e9da949a2a3 177 double new_pw;
pmmccorkell 0:6e9da949a2a3 178 double speed_pw=step_pw+(diff_pw*factor);
pmmccorkell 0:6e9da949a2a3 179 if (dir==1) new_pw=current_pw+speed_pw;
pmmccorkell 0:6e9da949a2a3 180 //new_pw=current_pw+.0000025;
pmmccorkell 0:6e9da949a2a3 181 if (dir==-1) new_pw=current_pw-speed_pw;
pmmccorkell 0:6e9da949a2a3 182 //new_pw=current_pw-.0000025;
pmmccorkell 0:6e9da949a2a3 183 pc.printf("diff: %f, factor: %f\r\n",diff_pw,factor);
pmmccorkell 0:6e9da949a2a3 184 pc.printf("pw: %f, speed: %f, offset: %f\r\n",current_pw,speed_pw,step_pw);
pmmccorkell 0:6e9da949a2a3 185 RH_motor.pulsewidth(new_pw);
pmmccorkell 0:6e9da949a2a3 186 wait_ms(dwell_time);
pmmccorkell 0:6e9da949a2a3 187 time_count+=dwell_time;
pmmccorkell 0:6e9da949a2a3 188 }
pmmccorkell 0:6e9da949a2a3 189 RH_motor.pulsewidth(target_pw);
pmmccorkell 0:6e9da949a2a3 190 //zero out Lm and Rm status bits
pmmccorkell 0:6e9da949a2a3 191 // ready=(ready & 0xffffffe7);
pmmccorkell 0:6e9da949a2a3 192 }
pmmccorkell 0:6e9da949a2a3 193
pmmccorkell 0:6e9da949a2a3 194 //Function to handle motor logic
pmmccorkell 0:6e9da949a2a3 195 void motors(int desired_heading) {
pmmccorkell 0:6e9da949a2a3 196 //Pull heading from BNO and correct for 16bit/deg resolution.
pmmccorkell 0:6e9da949a2a3 197 double current_heading=((data(0x1234abcd))/0x10);
pmmccorkell 0:6e9da949a2a3 198 //Set acceptable tolerance in deg, for heading accuracy.
pmmccorkell 0:6e9da949a2a3 199 double tolerance = 3.0;
pmmccorkell 0:6e9da949a2a3 200 //The natural #
pmmccorkell 0:6e9da949a2a3 201
pmmccorkell 0:6e9da949a2a3 202 //Error handling. Only continue for 0-360 range.
pmmccorkell 0:6e9da949a2a3 203 if (desired_heading>=0 and desired_heading<=360) {
pmmccorkell 0:6e9da949a2a3 204 //Calculate how far to turn in degrees.
pmmccorkell 0:6e9da949a2a3 205 double diff = abs(desired_heading-(current_heading));
pmmccorkell 0:6e9da949a2a3 206 //Correct for 360-0 edge cases if 'diff'erence is greater than 180.
pmmccorkell 0:6e9da949a2a3 207 //Change direction and recalculate for accurate 'tolerance' comparison.
pmmccorkell 0:6e9da949a2a3 208 if (diff>180) {
pmmccorkell 0:6e9da949a2a3 209 if (desired_heading>180) diff=((current_heading+180)-(desired_heading-180));
pmmccorkell 0:6e9da949a2a3 210 if (current_heading>180) diff=((desired_heading+180)-(current_heading-180));
pmmccorkell 0:6e9da949a2a3 211 }
pmmccorkell 0:6e9da949a2a3 212 while (diff > tolerance) {
pmmccorkell 0:6e9da949a2a3 213 double dir=1;
pmmccorkell 0:6e9da949a2a3 214 double speed=0;
pmmccorkell 0:6e9da949a2a3 215 current_heading=((data(0x1234abcd))/0x10);
pmmccorkell 0:6e9da949a2a3 216 diff=abs((desired_heading)-(current_heading));
pmmccorkell 0:6e9da949a2a3 217 //Sigmoid data
pmmccorkell 0:6e9da949a2a3 218 double center=15; //deg
pmmccorkell 0:6e9da949a2a3 219 double slope=0.5; //rate of change
pmmccorkell 0:6e9da949a2a3 220 if (diff>180) {
pmmccorkell 0:6e9da949a2a3 221 dir=(-1);
pmmccorkell 0:6e9da949a2a3 222 if (desired_heading>180) diff=((current_heading+180)-(desired_heading-180));
pmmccorkell 0:6e9da949a2a3 223 if (current_heading>180) diff=((desired_heading+180)-(current_heading-180));
pmmccorkell 0:6e9da949a2a3 224 }
pmmccorkell 0:6e9da949a2a3 225 if ((desired_heading-current_heading)<0) dir=(dir*-1);
pmmccorkell 0:6e9da949a2a3 226 speed=Sigmoid(esc_range,slope,center,diff);
pmmccorkell 0:6e9da949a2a3 227 if (dir==1) {
pmmccorkell 0:6e9da949a2a3 228 RH_motor_cntrl(stop_pw+speed);
pmmccorkell 0:6e9da949a2a3 229 TurnRight();
pmmccorkell 0:6e9da949a2a3 230 }
pmmccorkell 0:6e9da949a2a3 231 if (dir==-1) {
pmmccorkell 0:6e9da949a2a3 232 RH_motor_cntrl(stop_pw-speed);
pmmccorkell 0:6e9da949a2a3 233 TurnLeft();
pmmccorkell 0:6e9da949a2a3 234 }
pmmccorkell 0:6e9da949a2a3 235 wait_ms(20);
pmmccorkell 0:6e9da949a2a3 236 }
pmmccorkell 0:6e9da949a2a3 237 // pc.printf("heading: %i, diff: %i\r\n",current_heading,diff);
pmmccorkell 0:6e9da949a2a3 238 }
pmmccorkell 0:6e9da949a2a3 239 }
pmmccorkell 0:6e9da949a2a3 240
pmmccorkell 0:6e9da949a2a3 241 void maintain(int desired_heading) {
pmmccorkell 0:6e9da949a2a3 242 while(1) {
pmmccorkell 0:6e9da949a2a3 243 motors(desired_heading);
pmmccorkell 0:6e9da949a2a3 244 wait_ms(20);
pmmccorkell 0:6e9da949a2a3 245 }
pmmccorkell 0:6e9da949a2a3 246 }
pmmccorkell 0:6e9da949a2a3 247
pmmccorkell 0:6e9da949a2a3 248 //Function to accept desired position, program BNO,
pmmccorkell 0:6e9da949a2a3 249 // and update ready word by masking non-position bits
pmmccorkell 0:6e9da949a2a3 250 // and adding the new position to position bits
pmmccorkell 0:6e9da949a2a3 251 void switch_pos(int position) {
pmmccorkell 0:6e9da949a2a3 252 if (position>=0 and position<8) {
pmmccorkell 0:6e9da949a2a3 253 switch (position) {
pmmccorkell 0:6e9da949a2a3 254 case 1:
pmmccorkell 0:6e9da949a2a3 255 imu.set_mounting_position(MT_P1);
pmmccorkell 0:6e9da949a2a3 256 ready=((ready & 0xfffffff8)+0x1);
pmmccorkell 0:6e9da949a2a3 257 break;
pmmccorkell 0:6e9da949a2a3 258 case 2:
pmmccorkell 0:6e9da949a2a3 259 imu.set_mounting_position(MT_P2);
pmmccorkell 0:6e9da949a2a3 260 ready=((ready & 0xfffffff8)+0x2);
pmmccorkell 0:6e9da949a2a3 261 break;
pmmccorkell 0:6e9da949a2a3 262 case 3:
pmmccorkell 0:6e9da949a2a3 263 imu.set_mounting_position(MT_P3);
pmmccorkell 0:6e9da949a2a3 264 ready=((ready & 0xfffffff8)+0x3);
pmmccorkell 0:6e9da949a2a3 265 break;
pmmccorkell 0:6e9da949a2a3 266 case 4:
pmmccorkell 0:6e9da949a2a3 267 imu.set_mounting_position(MT_P4);
pmmccorkell 0:6e9da949a2a3 268 ready=((ready & 0xfffffff8)+0x4);
pmmccorkell 0:6e9da949a2a3 269 break;
pmmccorkell 0:6e9da949a2a3 270 case 5:
pmmccorkell 0:6e9da949a2a3 271 imu.set_mounting_position(MT_P5);
pmmccorkell 0:6e9da949a2a3 272 ready=((ready & 0xfffffff8)+0x5);
pmmccorkell 0:6e9da949a2a3 273 break;
pmmccorkell 0:6e9da949a2a3 274 case 6:
pmmccorkell 0:6e9da949a2a3 275 imu.set_mounting_position(MT_P6);
pmmccorkell 0:6e9da949a2a3 276 ready=((ready & 0xfffffff8)+0x6);
pmmccorkell 0:6e9da949a2a3 277 break;
pmmccorkell 0:6e9da949a2a3 278 case 7:
pmmccorkell 0:6e9da949a2a3 279 imu.set_mounting_position(MT_P7);
pmmccorkell 0:6e9da949a2a3 280 ready=((ready & 0xfffffff8)+0x7);
pmmccorkell 0:6e9da949a2a3 281 break;
pmmccorkell 0:6e9da949a2a3 282 case 0:
pmmccorkell 0:6e9da949a2a3 283 default:
pmmccorkell 0:6e9da949a2a3 284 imu.set_mounting_position(MT_P0);
pmmccorkell 0:6e9da949a2a3 285 ready=((ready & 0xfffffff8));
pmmccorkell 0:6e9da949a2a3 286 break;
pmmccorkell 0:6e9da949a2a3 287 }
pmmccorkell 0:6e9da949a2a3 288 }
pmmccorkell 0:6e9da949a2a3 289 }
pmmccorkell 0:6e9da949a2a3 290
pmmccorkell 0:6e9da949a2a3 291 //Commands function handles Serial input, checks for correct syntax, and calls appropriate function(s) to execute commands.
pmmccorkell 0:6e9da949a2a3 292 void commands() {
pmmccorkell 0:6e9da949a2a3 293 //Instantiate input char array to buffer Serial input.
pmmccorkell 0:6e9da949a2a3 294 char input[1000];
pmmccorkell 0:6e9da949a2a3 295 //check_pos char array used to filter Position commands.
pmmccorkell 0:6e9da949a2a3 296 char check_pos[5]="pos:";
pmmccorkell 0:6e9da949a2a3 297 //check_hea char array used to filter Go to Heading commands.
pmmccorkell 0:6e9da949a2a3 298 char check_hea[5]="hea:";
pmmccorkell 0:6e9da949a2a3 299 //check_mnt char array used to filter Maintain Heading commands.
pmmccorkell 0:6e9da949a2a3 300 char check_mnt[5]="mnt:";
pmmccorkell 0:6e9da949a2a3 301
pmmccorkell 0:6e9da949a2a3 302 //While loop reads Serial input into input buffer.
pmmccorkell 0:6e9da949a2a3 303 int i=0;
pmmccorkell 0:6e9da949a2a3 304 while (pc.readable()) {
pmmccorkell 0:6e9da949a2a3 305 input[i]=pc.getc();
pmmccorkell 0:6e9da949a2a3 306 // pc.printf("read char %c to %i\r\n",input[i], i);
pmmccorkell 0:6e9da949a2a3 307 i++;
pmmccorkell 0:6e9da949a2a3 308 }
pmmccorkell 0:6e9da949a2a3 309
pmmccorkell 0:6e9da949a2a3 310 //Only continue if input buffer has 7 entries, otherwise ignore input buffer.
pmmccorkell 0:6e9da949a2a3 311 //All commands from RasPi shall come in a format of 7 characters "abc:xyz"
pmmccorkell 0:6e9da949a2a3 312 // where 'abc' is an identifying string and 'xyz' is a decimal number.
pmmccorkell 0:6e9da949a2a3 313 if (i==7) {
pmmccorkell 0:6e9da949a2a3 314 // pc.printf("checking 0-%c, 1-%c, 2-%c, 3-%c, 4-%c \r\n",check_pos[0],check_pos[1],check_pos[2],check_pos[3],check_pos[4]);
pmmccorkell 0:6e9da949a2a3 315
pmmccorkell 0:6e9da949a2a3 316 //Instantiate counters that will be used to verify known commands.
pmmccorkell 0:6e9da949a2a3 317 int verified_pos=0;
pmmccorkell 0:6e9da949a2a3 318 int verified_hea=0;
pmmccorkell 0:6e9da949a2a3 319 int verified_mnt=0;
pmmccorkell 0:6e9da949a2a3 320
pmmccorkell 0:6e9da949a2a3 321 //While loop checks first 4 characters of input buffer and attempts to match
pmmccorkell 0:6e9da949a2a3 322 // against known commands.
pmmccorkell 0:6e9da949a2a3 323 int q=0;
pmmccorkell 0:6e9da949a2a3 324 while (q<4) {
pmmccorkell 0:6e9da949a2a3 325 //Increment verified_pos if a match is found between Serial input buffer
pmmccorkell 0:6e9da949a2a3 326 // and Position command format.
pmmccorkell 0:6e9da949a2a3 327 if (input[q]==check_pos[q]) {
pmmccorkell 0:6e9da949a2a3 328 verified_pos++;
pmmccorkell 0:6e9da949a2a3 329 // pc.printf("verified for pos %c at %i\r\n",input[q],q);
pmmccorkell 0:6e9da949a2a3 330 }
pmmccorkell 0:6e9da949a2a3 331
pmmccorkell 0:6e9da949a2a3 332 //Increment verified_hea if a match is found between Serial input buffer
pmmccorkell 0:6e9da949a2a3 333 // and Heading command format.
pmmccorkell 0:6e9da949a2a3 334 if (input[q]==check_hea[q]) {
pmmccorkell 0:6e9da949a2a3 335 // pc.printf("verified for hea %c at %i\r\n",input[q],q);
pmmccorkell 0:6e9da949a2a3 336 verified_hea++;
pmmccorkell 0:6e9da949a2a3 337 }
pmmccorkell 0:6e9da949a2a3 338
pmmccorkell 0:6e9da949a2a3 339 if (input[q]==check_mnt[q]) {
pmmccorkell 0:6e9da949a2a3 340 verified_mnt++;
pmmccorkell 0:6e9da949a2a3 341 }
pmmccorkell 0:6e9da949a2a3 342 q++;
pmmccorkell 0:6e9da949a2a3 343 }
pmmccorkell 0:6e9da949a2a3 344
pmmccorkell 0:6e9da949a2a3 345 //If first 4 characters from Serial input buffer match Position command format,
pmmccorkell 0:6e9da949a2a3 346 // execute "switch_pos" function.
pmmccorkell 0:6e9da949a2a3 347 if (verified_pos==4) {
pmmccorkell 0:6e9da949a2a3 348 int change=(input[6]-'0');
pmmccorkell 0:6e9da949a2a3 349 switch_pos(change);
pmmccorkell 0:6e9da949a2a3 350 }
pmmccorkell 0:6e9da949a2a3 351
pmmccorkell 0:6e9da949a2a3 352 //If first 4 characters from Serial input buffer match Heading command format,
pmmccorkell 0:6e9da949a2a3 353 // execute "motors" function.
pmmccorkell 0:6e9da949a2a3 354 if (verified_hea==4) {
pmmccorkell 0:6e9da949a2a3 355 //Correct for ascii '0', and reform 3digit decimal number
pmmccorkell 0:6e9da949a2a3 356 int hea100=(input[4]-'0');
pmmccorkell 0:6e9da949a2a3 357 int hea10=(input[5]-'0');
pmmccorkell 0:6e9da949a2a3 358 int hea1=(input[6]-'0');
pmmccorkell 0:6e9da949a2a3 359 int hea=(hea100*100)+(hea10*10)+(hea1*1);
pmmccorkell 0:6e9da949a2a3 360 // pc.printf("heading: %i\r\n",hea);
pmmccorkell 0:6e9da949a2a3 361 motors(hea);
pmmccorkell 0:6e9da949a2a3 362 //zero out ready status for LH and RH motors
pmmccorkell 0:6e9da949a2a3 363 ready=(ready & 0xffffffe7);
pmmccorkell 0:6e9da949a2a3 364 Stop_motors();
pmmccorkell 0:6e9da949a2a3 365 RH_motor_cntrl(stop_pw);
pmmccorkell 0:6e9da949a2a3 366 }
pmmccorkell 0:6e9da949a2a3 367
pmmccorkell 0:6e9da949a2a3 368 if (verified_mnt==4) {
pmmccorkell 0:6e9da949a2a3 369 int mnt100=(input[4]-'0');
pmmccorkell 0:6e9da949a2a3 370 int mnt10=(input[5]-'0');
pmmccorkell 0:6e9da949a2a3 371 int mnt1=(input[6]-'0');
pmmccorkell 0:6e9da949a2a3 372 int mnt=(mnt100*100)+(mnt10*10)+(mnt1*1);
pmmccorkell 0:6e9da949a2a3 373 maintain(mnt);
pmmccorkell 0:6e9da949a2a3 374 RH_motor_cntrl(stop_pw);
pmmccorkell 0:6e9da949a2a3 375 //zero out ready status for LH and RH motors
pmmccorkell 0:6e9da949a2a3 376 ready=(ready & 0xffffffe7);
pmmccorkell 0:6e9da949a2a3 377 }
pmmccorkell 0:6e9da949a2a3 378 }
pmmccorkell 0:6e9da949a2a3 379 }
pmmccorkell 0:6e9da949a2a3 380
pmmccorkell 0:6e9da949a2a3 381 int main() {
pmmccorkell 0:6e9da949a2a3 382 //engage plaidspeed
pmmccorkell 0:6e9da949a2a3 383 pc.baud(115200);
pmmccorkell 0:6e9da949a2a3 384 // imu.set_mounting_position(MT_P3);
pmmccorkell 0:6e9da949a2a3 385 RH_motor.period(.02);
pmmccorkell 0:6e9da949a2a3 386 RH_motor.pulsewidth(stop_pw);
pmmccorkell 0:6e9da949a2a3 387
pmmccorkell 0:6e9da949a2a3 388 //If not ready, reset BNO.
pmmccorkell 0:6e9da949a2a3 389 while (imu.chip_ready() == 0) {
pmmccorkell 0:6e9da949a2a3 390 do {
pmmccorkell 0:6e9da949a2a3 391 pc.printf("resetting BNO\r\n");
pmmccorkell 0:6e9da949a2a3 392 pwr_on=0;
pmmccorkell 0:6e9da949a2a3 393 wait_ms(100);
pmmccorkell 0:6e9da949a2a3 394 pwr_on=1;
pmmccorkell 0:6e9da949a2a3 395 wait_ms(50);
pmmccorkell 0:6e9da949a2a3 396 } while(imu.reset());
pmmccorkell 0:6e9da949a2a3 397 }
pmmccorkell 0:6e9da949a2a3 398 wait_ms(20);
pmmccorkell 0:6e9da949a2a3 399
pmmccorkell 0:6e9da949a2a3 400 //If BNO is ready, set ready status indication
pmmccorkell 0:6e9da949a2a3 401 if (imu.chip_ready()) {
pmmccorkell 0:6e9da949a2a3 402 ready=0xffff0000;
pmmccorkell 0:6e9da949a2a3 403 }
pmmccorkell 0:6e9da949a2a3 404 // uint32_t keyver=intake(ready);
pmmccorkell 0:6e9da949a2a3 405
pmmccorkell 0:6e9da949a2a3 406 //Look for serial input commands and send to 'commands' function.
pmmccorkell 0:6e9da949a2a3 407 //If no serial input commands, stream data.
pmmccorkell 0:6e9da949a2a3 408 while(1) {
pmmccorkell 0:6e9da949a2a3 409 if (pc.readable()) commands();
pmmccorkell 0:6e9da949a2a3 410 else data(0x1234abcd);
pmmccorkell 0:6e9da949a2a3 411 wait_ms(20);
pmmccorkell 0:6e9da949a2a3 412 }
pmmccorkell 0:6e9da949a2a3 413 }
pmmccorkell 0:6e9da949a2a3 414