Initial Commit
Diff: robot.cpp
- Revision:
- 1:282467cbebb3
- Parent:
- 0:b74b6d70347d
- Child:
- 2:37a19a9e5f2c
--- a/robot.cpp Sun Oct 05 12:21:03 2014 +0000 +++ b/robot.cpp Sat Oct 11 03:53:04 2014 +0000 @@ -12,8 +12,8 @@ //*********************************CONSTRUCTOR*********************************// HC05 bt(tx_bt,rx_bt,EN_BT); //QEI wheel (PTA16, PTA17, NC, 24); -QEI left (PTA16, PTA17, NC, 150, QEI::X4_ENCODING); -QEI right (PTA14, PTA13, NC, 150, QEI::X4_ENCODING); +QEI right (PTA16, PTA17, NC, 150, QEI::X4_ENCODING); +QEI left (PTA14, PTA13, NC, 150, QEI::X4_ENCODING); //Serial bt(rx_bt,tx_bt); //MPU6050 mpu(PTE0, PTE1); DigitalOut myled(myledd); @@ -33,38 +33,50 @@ DigitalOut BIN1(MOT_BIN1_PIN); DigitalOut BIN2(MOT_BIN2_PIN); DigitalOut STBY(MOT_STBY_PIN); -int rMotor = -1; + +DigitalOut SRX(PTB10); + +AnalogIn uL(ulL); +AnalogIn uF(ulF); +AnalogIn uR(ulR); +AnalogIn urR(ulrR); +AnalogIn urL(ulrL); +AnalogIn uB(ulB); +MPU6050 accelgyro; +int16_t ax, ay, az; +int16_t gx, gy, gz; +int rMotor = 1; int lMotor = -1; int m_speed = 100; int speed; Mutex stdio_mutex; int freq=0; -/* -double target_angle=0; -double rz; //Direction robot is facing -double Irz; //integral of the rotation offset from target. (Optionally) Used for PID control of direction -double angle_origin; //Angle of origin (can be changed later, or set if robot starts at known angle) -bool AUTO_ORIENT; //if this flag is 1, the robot automatically orients itself to selected direction -bool REMOTE_CONTROL; //if this flag is 1, the robot will be controlled over bluetooth -int acc[3]; -int gyr[3]; -bool MPU_OK; -double timeNext; -int speed; -int accdata[3]; //data from accelerometer (raw) -int gyrodata[3]; //data from gyro (raw) - //double gyroCorrect; //= 3720; //divide by this to get gyro data in RAD/SECOND. This is above as a #DEFINE -int gyroOffset[3]; //Correction value for each gyroscope to zero the values. -int accOffset[3]; //correction value for each accelerometer -double dx = 0; //The current displacement in the x-axis (side-side) -double dy = 0; //The current displacement in the y-axis (forward-back) -double dz = 0; //The current displacement in the z-axis (up-down) -double origin = 0; -int freq=0; -*/ +Timer t; +int heading=0; +int last_time=0; +int dy =0; +int dx=0; +float left_current_reading=0; +float right_current_reading= 0; +float left_change = 0; +float right_change =0; +float left_prev_read=0; +float right_prev_read=0; +int delta_y=0; +int delta_x=0; +float delta_thetha=0; +float encoder_yaw =0; +float G_thetha=0; +int Encoder_x=0; +int Encoder_y=0; +int dtheta=0; +int r_time () +{ + int mseconds = (int)time(NULL)+(int)t.read_ms(); + return mseconds; +} void initRobot() { - key = 0; //btSwitch = 1; bt.start(); @@ -72,6 +84,9 @@ wait_ms(500); bt.baud(BaudRate_bt); myled = 1; + accelgyro.initialize(); + t.start(); + SRX = 0; } //*********************************MOTORS*********************************// @@ -179,13 +194,63 @@ //bt.printf("s;%lf;s\n\r",pulse_delay); //bt.unlock(); buzzer=1; - Thread::wait(pulse_delay); + //Thread::wait(pulse_delay); buzzer=0; - Thread::wait(pulse_delay); + //Thread::wait(pulse_delay); } //freq = 150+(freq*10); //buzzer.period_us(1000000/freq); // 4 second period //buzz.pulsewidth(2); // 2 second pulse (on) //buzzer.write(0.5f); // 50% duty cycle +} +void Imu_yaw(void const *args) +{ +while(true) + +{ + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + int m_seconds = r_time(); + float dt = ((float)(m_seconds-last_time))/1000; + last_time=m_seconds; + if ((gz)<800&& gz>-800) { + gz=0; + } + stdio_mutex.lock(); + heading = heading + (dt*gz)*3/4/100; + if(heading>360) + heading= heading -360; + else if (heading <0) + heading = heading +360; + stdio_mutex.unlock(); + Thread:: wait(20); +} +} +void encoder_thread(void const *args) +{ +while(true) +{ + left_current_reading=left.getPulses()*(-1)/5; + right_current_reading= right.getPulses()/5; + left_change = left_current_reading- left_prev_read; + right_change =right_current_reading- right_prev_read; + left_prev_read=left_current_reading; + right_prev_read=right_current_reading; + delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2; + delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2; + delta_thetha=atan((right_change-left_change)/100); + encoder_yaw =encoder_yaw+delta_thetha; + G_thetha=encoder_yaw*180/M_PI; //global thetha, overall + Encoder_x=Encoder_x+delta_x; + Encoder_y=Encoder_y+delta_y; + stdio_mutex.lock(); + dx=delta_x+dx; + dy=delta_y+dy; + dtheta=delta_thetha*180/M_PI; + stdio_mutex.unlock(); + //bt.lock(); + //bt.printf("%0.2lf\t%0.2lf;\t%d;\t%d;\t%d;\t%d;\t%lf;\t%lf;\t :s\n\r",left_current_reading,right_current_reading,Encoder_x,Encoder_y,delta_y,delta_x,delta_thetha,G_thetha); + //bt.unlock(); +Thread:: wait(50); +} } \ No newline at end of file