Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

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?

UserRevisionLine numberNew 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