Yuxiang Yang / ros_openroach

Mbed side code that supports OpenRoACH communication with ROS (Robot Operating System)

Dependencies:   MPU6050IMU QEI RPCInterface TSL1401CL mbed-src

Fork of mbed_zumy_rpc by Austin Buchan

main.cpp@3:06820ff9a80c, 2018-05-21 (annotated)

Committer:
yxyang
Date:
Mon May 21 18:51:48 2018 +0000
Revision:
3:06820ff9a80c
Parent:
2:2e7ed3a34dd1 
Removing useless dependencies

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abuchan 0:966d81803039 1 #include "mbed.h"
abuchan 0:966d81803039 2 #include "SerialRPCInterface.h"
abuchan 0:966d81803039 3 #include "MPU6050.h"
yxyang 1:7b8696baf8ff 4 #include "TSL1401CL.h"
yxyang 2:2e7ed3a34dd1 5 #include <list>
yxyang 2:2e7ed3a34dd1 6
yxyang 2:2e7ed3a34dd1 7 // Sensor pins
yxyang 2:2e7ed3a34dd1 8 #define clk p16
yxyang 2:2e7ed3a34dd1 9 #define si p17
yxyang 2:2e7ed3a34dd1 10 #define adc p18
yxyang 2:2e7ed3a34dd1 11 #define revs_per_count 894
yxyang 2:2e7ed3a34dd1 12
yxyang 1:7b8696baf8ff 13 void steerImprovedPointTurns(int8_t line_pos);
yxyang 1:7b8696baf8ff 14 void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d);
yxyang 2:2e7ed3a34dd1 15 void readCamera(Arguments* input, Reply* output);
yxyang 2:2e7ed3a34dd1 16
abuchan 0:966d81803039 17 SerialRPCInterface SerialRPC(USBTX, USBRX, 115200);
abuchan 0:966d81803039 18
yxyang 2:2e7ed3a34dd1 19 float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, duty_cycle_left, duty_cycle_right;
yxyang 1:7b8696baf8ff 20 int line_pos, line_pos_previous, line_lost_time;
yxyang 2:2e7ed3a34dd1 21 //int r_enc, l_enc, l_phase, r_phase;
yxyang 2:2e7ed3a34dd1 22 TSL1401CL cam(clk, si, adc);
yxyang 1:7b8696baf8ff 23
yxyang 1:7b8696baf8ff 24 DigitalOut led1(LED1);
yxyang 1:7b8696baf8ff 25 DigitalOut led2(LED2);
yxyang 1:7b8696baf8ff 26 DigitalOut led3(LED3);
yxyang 1:7b8696baf8ff 27 DigitalOut led4(LED4);
yxyang 1:7b8696baf8ff 28 PwmOut m1_fwd(p21);
yxyang 1:7b8696baf8ff 29 PwmOut m1_back(p22);
yxyang 1:7b8696baf8ff 30 PwmOut m2_fwd(p23);
yxyang 1:7b8696baf8ff 31 PwmOut m2_back(p24);
abuchan 0:966d81803039 32 RPCVariable<float> rpc_accel_x(&accel_x, "accel_x");
abuchan 0:966d81803039 33 RPCVariable<float> rpc_accel_y(&accel_y, "accel_y");
abuchan 0:966d81803039 34 RPCVariable<float> rpc_accel_z(&accel_z, "accel_z");
yxyang 1:7b8696baf8ff 35 RPCVariable<float> rpc_gyro_x(&gyro_x, "gyro_x");
yxyang 1:7b8696baf8ff 36 RPCVariable<float> rpc_gyro_y(&gyro_y, "gyro_y");
yxyang 1:7b8696baf8ff 37 RPCVariable<float> rpc_gyro_z(&gyro_z, "gyro_z");
yxyang 2:2e7ed3a34dd1 38 RPCVariable<float> rpc_left_duty_cycle(&duty_cycle_left, "duty_cycle_left");
yxyang 2:2e7ed3a34dd1 39 RPCVariable<float> rpc_right_duty_cycle(&duty_cycle_right, "duty_cycle_right");
yxyang 1:7b8696baf8ff 40 RPCVariable<int> rpc_line_pos(&line_pos, "line_pos");
yxyang 2:2e7ed3a34dd1 41 //RPCFunction rpc_camera_reading(&readCamera, "readCamera");
yxyang 2:2e7ed3a34dd1 42 //RPCVariable<int> rpc_r_enc(&r_enc, "r_enc");
yxyang 2:2e7ed3a34dd1 43 //RPCVariable<int> rpc_l_enc(&l_enc, "l_enc");
yxyang 1:7b8696baf8ff 44 //QEI l_wheel (p29, p30, NC, 624);
yxyang 1:7b8696baf8ff 45 //QEI r_wheel (p11, p12, NC, 624);
abuchan 0:966d81803039 46
abuchan 0:966d81803039 47 MPU6050 mpu6050;
yxyang 1:7b8696baf8ff 48
yxyang 1:7b8696baf8ff 49 #define CAM_INTEGRATION_TIME 80
yxyang 1:7b8696baf8ff 50
yxyang 1:7b8696baf8ff 51 // Higher line threshold -> the sensor will only recognize larger changes in
yxyang 1:7b8696baf8ff 52 // brightness as a line edge
yxyang 1:7b8696baf8ff 53 #define LINE_THRESHOLD 10
yxyang 1:7b8696baf8ff 54 #define LINE_PRECISION 2
yxyang 1:7b8696baf8ff 55 #define LINE_CROP_AMOUNT 4
yxyang 1:7b8696baf8ff 56
yxyang 1:7b8696baf8ff 57 // These constants define the base pwm across the motors and how much the
yxyang 1:7b8696baf8ff 58 // controller
yxyang 1:7b8696baf8ff 59 // adjusts based on position of the line relative to the sensor
yxyang 1:7b8696baf8ff 60 #define SPEED_PWM 0.2
yxyang 1:7b8696baf8ff 61 #define TURN_SENS_INNER 1.5F
yxyang 1:7b8696baf8ff 62 #define TURN_SENS_OUTER 0.5F
yxyang 1:7b8696baf8ff 63
yxyang 1:7b8696baf8ff 64 // Defines data
yxyang 1:7b8696baf8ff 65 #define LINE_HIST_SIZE 1000
yxyang 1:7b8696baf8ff 66 #define LINE_END_TIME 2500
yxyang 1:7b8696baf8ff 67
yxyang 1:7b8696baf8ff 68 // Sensor pins
yxyang 1:7b8696baf8ff 69 #define clk p16
yxyang 1:7b8696baf8ff 70 #define si p17
yxyang 1:7b8696baf8ff 71 #define adc p18
abuchan 0:966d81803039 72
abuchan 0:966d81803039 73 int main() {
yxyang 1:7b8696baf8ff 74 /********** SENSOR SETUP **********/
yxyang 1:7b8696baf8ff 75 setLEDs(1, 1, 1, 1);
yxyang 2:2e7ed3a34dd1 76
abuchan 0:966d81803039 77 //Set up I2C
abuchan 0:966d81803039 78 i2c.frequency(400000); // use fast (400 kHz) I2C
abuchan 0:966d81803039 79
abuchan 0:966d81803039 80 volatile bool imu_ready = false;
abuchan 0:966d81803039 81
abuchan 0:966d81803039 82 wait_ms(100);
abuchan 0:966d81803039 83
abuchan 0:966d81803039 84 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
abuchan 0:966d81803039 85
abuchan 0:966d81803039 86 if (whoami == 0x68) // WHO_AM_I should always be 0x68
abuchan 0:966d81803039 87 {
abuchan 0:966d81803039 88 mpu6050.MPU6050SelfTest(SelfTest);
abuchan 0:966d81803039 89 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
abuchan 0:966d81803039 90 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
abuchan 0:966d81803039 91 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
abuchan 0:966d81803039 92 mpu6050.initMPU6050();
abuchan 0:966d81803039 93 mpu6050.getAres();
abuchan 0:966d81803039 94 mpu6050.getGres();
abuchan 0:966d81803039 95 imu_ready = true;
abuchan 0:966d81803039 96 }
abuchan 0:966d81803039 97 }
yxyang 2:2e7ed3a34dd1 98 //while(!cam.integrationReady());
yxyang 2:2e7ed3a34dd1 99 //cam.read();
abuchan 0:966d81803039 100
yxyang 1:7b8696baf8ff 101 setLEDs(0, 0, 0, 1);
yxyang 2:2e7ed3a34dd1 102 duty_cycle_left = 0; duty_cycle_right = 0;
abuchan 0:966d81803039 103 while(1) {
yxyang 2:2e7ed3a34dd1 104 /***** Read line sensor *****/
yxyang 2:2e7ed3a34dd1 105 //cam.read();
abuchan 0:966d81803039 106 wait_ms(10);
yxyang 2:2e7ed3a34dd1 107 m1_fwd.write(duty_cycle_left);
yxyang 2:2e7ed3a34dd1 108 m2_fwd.write(duty_cycle_right);
yxyang 2:2e7ed3a34dd1 109 //r_enc=r_wheel.getPulses();
yxyang 2:2e7ed3a34dd1 110 //l_enc=l_wheel.getPulses();
abuchan 0:966d81803039 111
yxyang 1:7b8696baf8ff 112 if (imu_ready) {
abuchan 0:966d81803039 113 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
abuchan 0:966d81803039 114 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
abuchan 0:966d81803039 115 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
abuchan 0:966d81803039 116
abuchan 0:966d81803039 117 // Now we'll calculate the accleration value into actual g's
abuchan 0:966d81803039 118 accel_x = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
abuchan 0:966d81803039 119 accel_y = (float)accelCount[1]*aRes - accelBias[1];
abuchan 0:966d81803039 120 accel_z = (float)accelCount[2]*aRes - accelBias[2];
abuchan 0:966d81803039 121
abuchan 0:966d81803039 122 // Calculate the gyro value into actual degrees per second
abuchan 0:966d81803039 123 gyro_x = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
abuchan 0:966d81803039 124 gyro_y = (float)gyroCount[1]*gRes - gyroBias[1];
abuchan 0:966d81803039 125 gyro_z = (float)gyroCount[2]*gRes - gyroBias[2];
abuchan 0:966d81803039 126 }
abuchan 0:966d81803039 127 }
yxyang 2:2e7ed3a34dd1 128 /*
yxyang 2:2e7ed3a34dd1 129 l_phase = l_enc % revs_per_count; r_phase = r_enc % revs_per_count;
yxyang 2:2e7ed3a34dd1 130 if ((l_phase < revs_per_count / 2) && (r_phase < revs_per_count / 2)) {
yxyang 2:2e7ed3a34dd1 131 setLEDs(1, 0, 1, 0);
yxyang 2:2e7ed3a34dd1 132 } else if ((l_phase < revs_per_count / 2) && (r_phase > revs_per_count / 2)) {
yxyang 2:2e7ed3a34dd1 133 setLEDs(1, 0, 0, 1);
yxyang 2:2e7ed3a34dd1 134 } else if ((l_phase > revs_per_count / 2) && (r_phase < revs_per_count / 2)) {
yxyang 2:2e7ed3a34dd1 135 setLEDs(0, 1, 1, 0);
yxyang 2:2e7ed3a34dd1 136 } else {
yxyang 2:2e7ed3a34dd1 137 setLEDs(0, 1, 0, 1);
yxyang 2:2e7ed3a34dd1 138 }*/
abuchan 0:966d81803039 139 }
yxyang 1:7b8696baf8ff 140 }
yxyang 1:7b8696baf8ff 141
yxyang 1:7b8696baf8ff 142 void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
yxyang 1:7b8696baf8ff 143 led1 = a;
yxyang 1:7b8696baf8ff 144 led2 = b;
yxyang 1:7b8696baf8ff 145 led3 = c;
yxyang 1:7b8696baf8ff 146 led4 = d;
yxyang 1:7b8696baf8ff 147 }
yxyang 2:2e7ed3a34dd1 148 /*
yxyang 2:2e7ed3a34dd1 149 void readCamera(Arguments* input, Reply* output){
yxyang 2:2e7ed3a34dd1 150 uint32_t pos = input->getArg<int>();
yxyang 2:2e7ed3a34dd1 151 int ans = 0;
yxyang 2:2e7ed3a34dd1 152 setLEDs(1, 0, 0, 1);
yxyang 2:2e7ed3a34dd1 153 //if (pos < 128u){
yxyang 2:2e7ed3a34dd1 154 ans = cam.getData(pos);
yxyang 2:2e7ed3a34dd1 155 //}
yxyang 2:2e7ed3a34dd1 156 output->putData(ans);
yxyang 2:2e7ed3a34dd1 157 setLEDs(0, 1, 1, 0);
yxyang 2:2e7ed3a34dd1 158 }*/