Code for our FYDP -only one IMU works right now -RTOS is working
bt_shell/machine_interface/py_machine_interface.h@0:964eb6a2ef00, 2015-03-18 (annotated)
- Committer:
- majik
- Date:
- Wed Mar 18 22:23:48 2015 +0000
- Revision:
- 0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
majik | 0:964eb6a2ef00 | 1 | #ifndef PY_MACHINE_INTERFACE_H |
majik | 0:964eb6a2ef00 | 2 | #define PY_MACHINE_INTERFACE_H |
majik | 0:964eb6a2ef00 | 3 | |
majik | 0:964eb6a2ef00 | 4 | /**This file contains the implementations for the controls through a python interface |
majik | 0:964eb6a2ef00 | 5 | //The brain file uses ';' to separate lines |
majik | 0:964eb6a2ef00 | 6 | Read IMU [acceleration and gyros] |
majik | 0:964eb6a2ef00 | 7 | Read Gyro |
majik | 0:964eb6a2ef00 | 8 | Read Angle |
majik | 0:964eb6a2ef00 | 9 | Read Acceleration |
majik | 0:964eb6a2ef00 | 10 | Read IR sensors |
majik | 0:964eb6a2ef00 | 11 | Read Optical flow sensors |
majik | 0:964eb6a2ef00 | 12 | Read current sensor |
majik | 0:964eb6a2ef00 | 13 | Read voltage sensor |
majik | 0:964eb6a2ef00 | 14 | |
majik | 0:964eb6a2ef00 | 15 | Drive to [x,y] (x,y) |
majik | 0:964eb6a2ef00 | 16 | Drive fwd (speed, time) |
majik | 0:964eb6a2ef00 | 17 | Run R motor (speed,time) |
majik | 0:964eb6a2ef00 | 18 | Run L motor (speed,time) |
majik | 0:964eb6a2ef00 | 19 | Set Direction (direction) |
majik | 0:964eb6a2ef00 | 20 | Enable IMU control (bool) //if this is enabled, IMU will keep robot straight |
majik | 0:964eb6a2ef00 | 21 | */ |
majik | 0:964eb6a2ef00 | 22 | #include "keybindings.h" |
majik | 0:964eb6a2ef00 | 23 | |
majik | 0:964eb6a2ef00 | 24 | int get_cmd(); //this reads from the bt connection |
majik | 0:964eb6a2ef00 | 25 | int q_pow10(int n); //this calculates 10^n quickly (only up to 10^6) |
majik | 0:964eb6a2ef00 | 26 | |
majik | 0:964eb6a2ef00 | 27 | char C; |
majik | 0:964eb6a2ef00 | 28 | char buffer[10]; |
majik | 0:964eb6a2ef00 | 29 | |
majik | 0:964eb6a2ef00 | 30 | |
majik | 0:964eb6a2ef00 | 31 | bool python_shell_connection(char c) |
majik | 0:964eb6a2ef00 | 32 | { |
majik | 0:964eb6a2ef00 | 33 | float dum; |
majik | 0:964eb6a2ef00 | 34 | int arg_in1; |
majik | 0:964eb6a2ef00 | 35 | int arg_in2; |
majik | 0:964eb6a2ef00 | 36 | switch (c) |
majik | 0:964eb6a2ef00 | 37 | { |
majik | 0:964eb6a2ef00 | 38 | case CONNECTION: //Return query if connection is ready |
majik | 0:964eb6a2ef00 | 39 | bt.printf("\nEBOTDEMO\n"); |
majik | 0:964eb6a2ef00 | 40 | break; |
majik | 0:964eb6a2ef00 | 41 | |
majik | 0:964eb6a2ef00 | 42 | case CURRENT_SENSE: |
majik | 0:964eb6a2ef00 | 43 | dum = current_sensor.get_current(); |
majik | 0:964eb6a2ef00 | 44 | bt.printf("I%3.3f;",dum); |
majik | 0:964eb6a2ef00 | 45 | break; |
majik | 0:964eb6a2ef00 | 46 | case IMU_READ: |
majik | 0:964eb6a2ef00 | 47 | bt.printf("i;%3.3f;%3.3f;%3.3f;%3.3f;%3.3f;%3.3f;",imu_data.acc[0],imu_data.acc[1],imu_data.acc[2],imu_data.ypr[0],imu_data.ypr[1],imu_data.ypr[2]); |
majik | 0:964eb6a2ef00 | 48 | break; |
majik | 0:964eb6a2ef00 | 49 | case IMU_YAW: |
majik | 0:964eb6a2ef00 | 50 | dum = imu_data.ypr[0]*180/PI; |
majik | 0:964eb6a2ef00 | 51 | bt.printf("y;%3.3f;",dum); |
majik | 0:964eb6a2ef00 | 52 | break; |
majik | 0:964eb6a2ef00 | 53 | //optical flow |
majik | 0:964eb6a2ef00 | 54 | case OPT_READ: |
majik | 0:964eb6a2ef00 | 55 | //return optical flow (pixels/second for left and right) |
majik | 0:964eb6a2ef00 | 56 | //TODO: include second optical flow sensor |
majik | 0:964eb6a2ef00 | 57 | //bt.printf("O;%3.3f;%3.3f;",opt_flow.dx_px,opt_flow.dy_px); |
majik | 0:964eb6a2ef00 | 58 | break; |
majik | 0:964eb6a2ef00 | 59 | case IR_READ: |
majik | 0:964eb6a2ef00 | 60 | //return 5 IR sensors clockwise (Front, right_front, right_rear, left_rear, left_front) |
majik | 0:964eb6a2ef00 | 61 | //ask Wen to fill out this function |
majik | 0:964eb6a2ef00 | 62 | |
majik | 0:964eb6a2ef00 | 63 | |
majik | 0:964eb6a2ef00 | 64 | break; |
majik | 0:964eb6a2ef00 | 65 | case LOCATION: |
majik | 0:964eb6a2ef00 | 66 | //return x,y,z,theta (maybe not z) |
majik | 0:964eb6a2ef00 | 67 | break; |
majik | 0:964eb6a2ef00 | 68 | /*case MOTOR_R: |
majik | 0:964eb6a2ef00 | 69 | arg_in1 = get_cmd(); |
majik | 0:964eb6a2ef00 | 70 | *motors.Right = arg_in1; |
majik | 0:964eb6a2ef00 | 71 | case MOTOR_L: |
majik | 0:964eb6a2ef00 | 72 | arg_in1 = get_cmd(); |
majik | 0:964eb6a2ef00 | 73 | *motors.Left = arg_in1; |
majik | 0:964eb6a2ef00 | 74 | */ |
majik | 0:964eb6a2ef00 | 75 | case MOTOR: //command: 'w[L_speed],[R_speed] //I should write an actual function to receive data from serial |
majik | 0:964eb6a2ef00 | 76 | //Another argument should be included: time to run motors. |
majik | 0:964eb6a2ef00 | 77 | arg_in1 = get_cmd(); |
majik | 0:964eb6a2ef00 | 78 | arg_in2 = get_cmd(); |
majik | 0:964eb6a2ef00 | 79 | *motors.Right = arg_in2; |
majik | 0:964eb6a2ef00 | 80 | *motors.Left = arg_in1; |
majik | 0:964eb6a2ef00 | 81 | bt.printf("\tL=%d\tR=%d;",arg_in1,arg_in2); |
majik | 0:964eb6a2ef00 | 82 | break; |
majik | 0:964eb6a2ef00 | 83 | case (MOTOR_STOP): |
majik | 0:964eb6a2ef00 | 84 | *motors.Right = 0; |
majik | 0:964eb6a2ef00 | 85 | *motors.Left = 0; |
majik | 0:964eb6a2ef00 | 86 | break; |
majik | 0:964eb6a2ef00 | 87 | case END_CONNECTION: |
majik | 0:964eb6a2ef00 | 88 | bt.printf("End Connection\n"); |
majik | 0:964eb6a2ef00 | 89 | return 0; |
majik | 0:964eb6a2ef00 | 90 | ///break; |
majik | 0:964eb6a2ef00 | 91 | default: |
majik | 0:964eb6a2ef00 | 92 | bt.putc(c); //echo command |
majik | 0:964eb6a2ef00 | 93 | break; |
majik | 0:964eb6a2ef00 | 94 | } |
majik | 0:964eb6a2ef00 | 95 | bt.printf("\n\r"); |
majik | 0:964eb6a2ef00 | 96 | return 1; |
majik | 0:964eb6a2ef00 | 97 | } |
majik | 0:964eb6a2ef00 | 98 | |
majik | 0:964eb6a2ef00 | 99 | void interface_send_data(interface x) |
majik | 0:964eb6a2ef00 | 100 | { |
majik | 0:964eb6a2ef00 | 101 | bt.printf("#%f;",t.read()); //indicate start of line, output system time |
majik | 0:964eb6a2ef00 | 102 | //send all the requested data |
majik | 0:964eb6a2ef00 | 103 | if (x.list[0]){ //send all imu data |
majik | 0:964eb6a2ef00 | 104 | //bt.printf("A%3.3f,%3.3f,%3.3f,G%3.3f,%3.3f,%3.3f",mpu.ax,mpu.ay,mpu.az,mpu.gx,mpu.gy,mpu.gz); |
majik | 0:964eb6a2ef00 | 105 | } |
majik | 0:964eb6a2ef00 | 106 | |
majik | 0:964eb6a2ef00 | 107 | //0 - IMU_all |
majik | 0:964eb6a2ef00 | 108 | //1 - IMU_acc |
majik | 0:964eb6a2ef00 | 109 | //2 - IMU_gyr |
majik | 0:964eb6a2ef00 | 110 | //3 - IMU_rotation |
majik | 0:964eb6a2ef00 | 111 | //4 - IR_sensors |
majik | 0:964eb6a2ef00 | 112 | //5 - Optical_flow_sensors |
majik | 0:964eb6a2ef00 | 113 | //6 - Current_sensor |
majik | 0:964eb6a2ef00 | 114 | //7 - Voltage_sensor |
majik | 0:964eb6a2ef00 | 115 | } |
majik | 0:964eb6a2ef00 | 116 | |
majik | 0:964eb6a2ef00 | 117 | int get_cmd() //this requires a comma at the end of every command |
majik | 0:964eb6a2ef00 | 118 | { |
majik | 0:964eb6a2ef00 | 119 | char c = '0'; |
majik | 0:964eb6a2ef00 | 120 | char C[10]; //stores the array of chars read |
majik | 0:964eb6a2ef00 | 121 | int n=0; //counter |
majik | 0:964eb6a2ef00 | 122 | int timeout = 0; //timeout counter |
majik | 0:964eb6a2ef00 | 123 | int accum = 0; //accumulates value of array |
majik | 0:964eb6a2ef00 | 124 | |
majik | 0:964eb6a2ef00 | 125 | while(c != ',' && c != 13 && c !=';'){ //keep looping until a comma/semicolon is read (or enter is pressed) |
majik | 0:964eb6a2ef00 | 126 | while(! bt.readable()){ |
majik | 0:964eb6a2ef00 | 127 | Thread::wait(20); //wait for char input |
majik | 0:964eb6a2ef00 | 128 | timeout++; |
majik | 0:964eb6a2ef00 | 129 | if(timeout > 100) |
majik | 0:964eb6a2ef00 | 130 | return -1; |
majik | 0:964eb6a2ef00 | 131 | } |
majik | 0:964eb6a2ef00 | 132 | timeout = 0; |
majik | 0:964eb6a2ef00 | 133 | c = bt.getc(); //read the next char in the buffer |
majik | 0:964eb6a2ef00 | 134 | C[n] = c; |
majik | 0:964eb6a2ef00 | 135 | n++; |
majik | 0:964eb6a2ef00 | 136 | if(c == '.' || n >=9) break; //concatenate decimals. I'm not handling decimals. No reason. |
majik | 0:964eb6a2ef00 | 137 | } |
majik | 0:964eb6a2ef00 | 138 | for(int i = 0; i<n-1; i++){ |
majik | 0:964eb6a2ef00 | 139 | accum += (C[i]-48)* q_pow10(n-i-2); //need to subtract 2: 1 b/c n is incremented at the end of each iteration, 2 b/c comma is read. |
majik | 0:964eb6a2ef00 | 140 | } |
majik | 0:964eb6a2ef00 | 141 | return accum; |
majik | 0:964eb6a2ef00 | 142 | } |
majik | 0:964eb6a2ef00 | 143 | /*int q_pow10(int n){ //returns 10^n, up to 10^7 [8 sig figs] |
majik | 0:964eb6a2ef00 | 144 | static int pow10[8] = {1,10,100,1000,10000,100000,1000000,10000000}; |
majik | 0:964eb6a2ef00 | 145 | return pow10[n]; |
majik | 0:964eb6a2ef00 | 146 | }*/ |
majik | 0:964eb6a2ef00 | 147 | |
majik | 0:964eb6a2ef00 | 148 | #endif |