Diff: main.cpp

Revision:

0:6e9da949a2a3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 18 11:14:10 2019 +0000
@@ -0,0 +1,414 @@
+#include "mbed.h"
+#include "BNO055.h"
+
+Serial pc(USBTX, USBRX);
+I2C i2c(p9,p10);
+DigitalOut pwr_on(p30);
+BNO055 imu(i2c,p26);
+//DigitalOut led1(LED1);
+
+//instantiate global ready to 0
+uint32_t ready = 0x00000000;
+
+BNO055_ID_INF_TypeDef       bno055_id_inf;
+BNO055_EULER_TypeDef        euler_angles;
+//BNO055_QUATERNION_TypeDef   quaternion;
+//BNO055_LIN_ACC_TypeDef      linear_acc;
+BNO055_GRAVITY_TypeDef      gravity;
+//BNO055_TEMPERATURE_TypeDef  chip_temp;
+
+// Instantiate motor driver PWM pins, duty cycle, and period
+double stop_pw=.0015;
+//double fwd_pw=.0018;
+//double rev_pw=.0012;
+PwmOut RH_motor(p23);
+PwmOut Lfwd(p21); // left forward
+PwmOut Lrev(p22); // left reverse
+PwmOut Rfwd(p24); // right forward
+PwmOut Rrev(p25); // right reverse
+float dutycycle=0.40;
+
+
+//Lazy... don't want to track these down everywhere
+    //Standard servo rate -> 50Hz
+double servo_freq=50;
+double servo_period=(1/servo_freq);
+    //25us per data sheet Blue Robotics BESC-R1
+double esc_step=(25/1000000);
+    //400Hz update frequency per data sheet
+int esc_freq=400;
+double esc_time_ms=(1000/esc_freq);
+    //1.1-1.9ms pw per data sheet
+double esc_range=.0004;
+    //natural #
+double e=2.71828;
+
+
+
+
+//Data function pulls data from BNO and sends over serial
+//Accepts unsigned 32bit key word (k) that RasPi will use as trigger
+uint16_t data(uint32_t k) {
+    //Instantiate status array of 6 32-bit words.
+    //First 16 bits of each 32-bit word are Identifiers for RasPi to correctly assign the trailing 16 bits of data.
+    uint32_t status[6]={0};
+
+    //word 0: Key
+        //Used to ensure Pi and Mbed are on same page.
+    status[0]=k;
+
+    //word 1: Status information.
+        //0xffff acts as prefix to identify Status for RasPi.
+        //Last 3 bits (from right) are current position (POS[0-7]). See BNO datasheet.
+        //4th bit (from right) is RH turn motors enabled.
+        //5th bit (from right) is LH turn motors enabled.
+    status[1]=ready;
+
+    //word 2: Calibration.
+        //0xc000 acts as prefix to identify Cal for RasPi.
+    status[2]=0xc0000000+imu.read_calib_status();
+
+    //Get Euler data from BNO.
+    imu.get_Euler_Angles(&euler_angles);
+
+    //word 3 is Heading.
+        //0xc100 acts as prefix to identify Heading for RasPi. 
+    uint16_t h = euler_angles.h;
+    status[3]=0xc1000000+h;
+
+    //Offset calculation: 360*16bit resolution = 5760 -> converted to hex = 0x1680
+    int offset=0x1680;
+
+    //word 4 is Roll.
+        //0xc300 acts as prefix to identify Roll for RasPi.
+        //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
+    uint16_t r = offset + euler_angles.r;
+    status[4]=0xc3000000+r;
+    
+    //word 5 is Pitch.
+        //0xc500 acts as prefix to identify Pitch for RasPi.
+        //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
+    uint16_t p = offset + euler_angles.p;
+    status[5]=0xc5000000+p;
+
+    //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi.
+    int i;
+    int l=(sizeof(status)/sizeof(uint32_t))-1;
+    for (i=0; i<=l; i++) {
+        pc.printf("%x\n", status[i]);
+  }
+ 
+  //return heading
+  return h;
+}
+
+//Function to setup motor PWM for right turn
+void TurnRight() {
+    //mask off other bits and set Rmotor status to on
+    ready=(ready & 0xfffffff7) + 0x8;
+//    pc.printf("Turning Right\r\n");
+    Rfwd.write(dutycycle);
+    Lrev.write(dutycycle);
+}
+
+//Function to setup motor PWM for left turn
+void TurnLeft() {
+    //mask off other bits and set Lmotor status to on
+    ready=(ready & 0xffffffef) + 0x10;
+//  pc.printf("Turning Left\r\n");
+    Lfwd.write(dutycycle);
+    Rrev.write(dutycycle);
+}
+
+//Function to stop all motors
+void Stop_motors() {
+    //Set PW on all PWM pins to 0
+    Lfwd.write(0);
+    Lrev.write(0);
+    Rfwd.write(0);
+    Rrev.write(0);
+    //zero out Lm and Rm status bits
+    ready=(ready & 0xffffffe7);
+}
+
+//The sigmoid curve for logistic function.
+//Used for continuous PWM and softstart / softstop near end points.
+double Sigmoid(double Ymax, double m, double x0, double x) {
+//  double Logistic_func=(Ymax/(1+(e^((-1*m)*(x-x0)))));
+    double exponent=((-1*m)*(x-x0));
+    double power=pow(e,exponent);
+    double Logistic_func=Ymax/(1+power);
+//  pc.printf("exp: %f, power:%f, ret: %f\r\n",exponent,power,Logistic_func);
+    return Logistic_func;
+}
+
+//Function to control RH motor
+void RH_motor_cntrl(double target_pw) {
+    double step_pw=.00005;
+    double current_pw=(RH_motor.read()*.02);
+    double diff_pw=abs(target_pw-current_pw);
+    //Time we want to take to get from 1.5 to [1.9 or 1.1]
+//  double Delay=0.5; //500ms
+    //Total # of steps / resolution of ESC
+//  double Steps=((.0019-.0015)/esc_step);
+    //Center of sigmoid curve
+    double Center=.1; //100ms -> ramp up within 200ms
+    //slope at Center
+    double Slope=50; //rate of ramp up
+    double time_count=0; //total time in while loop
+    double dwell_time=0.001; //time to wait between while loop
+    double tolerance_pw=.000045; //45us
+    double dir=0;
+    while(diff_pw>tolerance_pw) {
+        current_pw=(RH_motor.read()*.02);
+        diff_pw=abs(target_pw-current_pw);
+        if ((target_pw-current_pw)>=0) {
+            dir=(1);
+            //RH moving fwd
+//            ready=(ready & 0xfffffff7) + 0x8;
+        }
+        if ((target_pw-current_pw)<0) {
+            dir=(-1);
+            //RH moving rev
+//            ready=(ready & 0xffffffef) + 0x10;
+        }
+        double factor=Sigmoid(1,Slope,Center,time_count);
+        //double new_pw=stop_pw+(diff_pw*factor);
+        double new_pw;
+        double speed_pw=step_pw+(diff_pw*factor);
+        if (dir==1) new_pw=current_pw+speed_pw;
+            //new_pw=current_pw+.0000025;
+        if (dir==-1) new_pw=current_pw-speed_pw;
+            //new_pw=current_pw-.0000025;
+        pc.printf("diff: %f, factor: %f\r\n",diff_pw,factor);
+        pc.printf("pw: %f, speed: %f, offset: %f\r\n",current_pw,speed_pw,step_pw);
+        RH_motor.pulsewidth(new_pw);
+        wait_ms(dwell_time);
+        time_count+=dwell_time;
+    }
+    RH_motor.pulsewidth(target_pw);
+    //zero out Lm and Rm status bits
+//    ready=(ready & 0xffffffe7);
+}
+
+//Function to handle motor logic
+void motors(int desired_heading) {
+    //Pull heading from BNO and correct for 16bit/deg resolution.
+    double current_heading=((data(0x1234abcd))/0x10);
+    //Set acceptable tolerance in deg, for heading accuracy.
+    double tolerance = 3.0;
+    //The natural #
+
+    //Error handling. Only continue for 0-360 range.
+    if (desired_heading>=0 and desired_heading<=360) {
+        //Calculate how far to turn in degrees.
+        double diff = abs(desired_heading-(current_heading));
+        //Correct for 360-0 edge cases if 'diff'erence is greater than 180.
+        //Change direction and recalculate for accurate 'tolerance' comparison.
+        if (diff>180) {
+            if (desired_heading>180) diff=((current_heading+180)-(desired_heading-180));
+            if (current_heading>180) diff=((desired_heading+180)-(current_heading-180));
+        }
+        while (diff > tolerance) {
+            double dir=1;
+            double speed=0;
+            current_heading=((data(0x1234abcd))/0x10);
+            diff=abs((desired_heading)-(current_heading));
+            //Sigmoid data
+            double center=15; //deg
+            double slope=0.5; //rate of change
+            if (diff>180) {
+                dir=(-1);
+                if (desired_heading>180) diff=((current_heading+180)-(desired_heading-180));
+                if (current_heading>180) diff=((desired_heading+180)-(current_heading-180));
+            }
+            if ((desired_heading-current_heading)<0) dir=(dir*-1);
+            speed=Sigmoid(esc_range,slope,center,diff);
+            if (dir==1) {
+                RH_motor_cntrl(stop_pw+speed);
+                TurnRight();
+            }
+            if (dir==-1) {
+                RH_motor_cntrl(stop_pw-speed);
+                TurnLeft();
+            }
+            wait_ms(20);
+        }
+//      pc.printf("heading: %i, diff: %i\r\n",current_heading,diff);
+    }
+}
+
+void maintain(int desired_heading) {
+    while(1) {
+        motors(desired_heading);
+        wait_ms(20);
+    }
+}
+
+//Function to accept desired position, program BNO,
+//  and update ready word by masking non-position bits
+//  and adding the new position to position bits
+void switch_pos(int position) {
+    if (position>=0 and position<8) {
+        switch (position) {
+            case 1:
+                imu.set_mounting_position(MT_P1);
+                ready=((ready & 0xfffffff8)+0x1);
+                break;
+            case 2:
+                imu.set_mounting_position(MT_P2);
+                ready=((ready & 0xfffffff8)+0x2);
+                break;
+            case 3:
+                imu.set_mounting_position(MT_P3);
+                ready=((ready & 0xfffffff8)+0x3);
+                break;
+            case 4:
+                imu.set_mounting_position(MT_P4);
+                ready=((ready & 0xfffffff8)+0x4);
+                break;
+            case 5:
+                imu.set_mounting_position(MT_P5);
+                ready=((ready & 0xfffffff8)+0x5);
+                break;
+            case 6:
+                imu.set_mounting_position(MT_P6);
+                ready=((ready & 0xfffffff8)+0x6);
+                break;
+            case 7:
+                imu.set_mounting_position(MT_P7);
+                ready=((ready & 0xfffffff8)+0x7);
+                break;
+            case 0:
+            default:
+                imu.set_mounting_position(MT_P0);
+                ready=((ready & 0xfffffff8));
+                break;
+        }
+    }
+}
+
+//Commands function handles Serial input, checks for correct syntax, and calls appropriate function(s) to execute commands.
+void commands() {
+    //Instantiate input char array to buffer Serial input.
+    char input[1000];
+    //check_pos char array used to filter Position commands.
+    char check_pos[5]="pos:";
+    //check_hea char array used to filter Go to Heading commands.
+    char check_hea[5]="hea:";
+    //check_mnt char array used to filter Maintain Heading commands.
+    char check_mnt[5]="mnt:";
+
+    //While loop reads Serial input into input buffer.
+    int i=0;
+    while (pc.readable()) {
+        input[i]=pc.getc();
+//        pc.printf("read char %c to %i\r\n",input[i], i);
+        i++;
+    }
+
+    //Only continue if input buffer has 7 entries, otherwise ignore input buffer. 
+    //All commands from RasPi shall come in a format of 7 characters "abc:xyz" 
+    //      where 'abc' is an identifying string and 'xyz' is a decimal number.
+    if (i==7) {
+//        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]);
+
+        //Instantiate counters that will be used to verify known commands.
+        int verified_pos=0;
+        int verified_hea=0;
+        int verified_mnt=0;
+        
+        //While loop checks first 4 characters of input buffer and attempts to match
+        //      against known commands.
+        int q=0;
+        while (q<4) {
+            //Increment verified_pos if a match is found between Serial input buffer
+            //      and Position command format.
+            if (input[q]==check_pos[q]) {
+                verified_pos++;
+//                pc.printf("verified for pos %c at %i\r\n",input[q],q);
+            }
+            
+            //Increment verified_hea if a match is found between Serial input buffer
+            //      and Heading command format.
+            if (input[q]==check_hea[q]) {
+//                pc.printf("verified for hea %c at %i\r\n",input[q],q);
+                verified_hea++;
+            }
+            
+            if (input[q]==check_mnt[q]) {
+                verified_mnt++;
+            }
+            q++;
+        }
+        
+        //If first 4 characters from Serial input buffer match Position command format,
+        //      execute "switch_pos" function.
+        if (verified_pos==4) {
+            int change=(input[6]-'0');
+            switch_pos(change);
+        }
+        
+        //If first 4 characters from Serial input buffer match Heading command format,
+        //      execute "motors" function.
+        if (verified_hea==4) {
+            //Correct for ascii '0', and reform 3digit decimal number
+            int hea100=(input[4]-'0');
+            int hea10=(input[5]-'0');
+            int hea1=(input[6]-'0');
+            int hea=(hea100*100)+(hea10*10)+(hea1*1);
+//            pc.printf("heading: %i\r\n",hea);
+            motors(hea);
+            //zero out ready status for LH and RH motors
+            ready=(ready & 0xffffffe7);
+            Stop_motors();
+            RH_motor_cntrl(stop_pw);
+        }
+        
+        if (verified_mnt==4) {
+            int mnt100=(input[4]-'0');
+            int mnt10=(input[5]-'0');
+            int mnt1=(input[6]-'0');
+            int mnt=(mnt100*100)+(mnt10*10)+(mnt1*1);
+            maintain(mnt);
+            RH_motor_cntrl(stop_pw);
+            //zero out ready status for LH and RH motors
+            ready=(ready & 0xffffffe7);
+        }
+    }
+}
+
+int main() {
+    //engage plaidspeed
+    pc.baud(115200);
+//    imu.set_mounting_position(MT_P3);
+    RH_motor.period(.02);
+    RH_motor.pulsewidth(stop_pw);
+
+    //If not ready, reset BNO.
+    while (imu.chip_ready() == 0) {
+        do {
+            pc.printf("resetting BNO\r\n");
+            pwr_on=0;
+            wait_ms(100);
+            pwr_on=1;
+            wait_ms(50);
+        } while(imu.reset());
+    }
+    wait_ms(20);
+
+    //If BNO is ready, set ready status indication
+    if (imu.chip_ready()) {
+        ready=0xffff0000;
+    }
+//    uint32_t keyver=intake(ready);
+
+    //Look for serial input commands and send to 'commands' function.
+    //If no serial input commands, stream data.
+    while(1) {
+        if (pc.readable()) commands();
+        else data(0x1234abcd);
+        wait_ms(20);
+    }
+}
+