Initial Commit
Diff: robot.cpp
- Revision:
- 2:37a19a9e5f2c
- Parent:
- 1:282467cbebb3
- Child:
- 3:3e3314102e56
--- a/robot.cpp Sat Oct 11 03:53:04 2014 +0000 +++ b/robot.cpp Sun Oct 12 13:33:06 2014 +0000 @@ -46,10 +46,10 @@ int16_t ax, ay, az; int16_t gx, gy, gz; int rMotor = 1; -int lMotor = -1; +int lMotor = 1; int m_speed = 100; int speed; -Mutex stdio_mutex; +Mutex stdio_mutex; int freq=0; Timer t; int heading=0; @@ -70,6 +70,7 @@ int Encoder_x=0; int Encoder_y=0; int dtheta=0; +int software_interuupt; int r_time () { int mseconds = (int)time(NULL)+(int)t.read_ms(); @@ -93,10 +94,9 @@ void motor_control(int Lspeed, int Rspeed) { //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed - if (!Lspeed && !Rspeed) //stop// - { STBY = 0; - } - else + if (!Lspeed && !Rspeed) { //stop// + STBY = 0; + } else STBY = 1; //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10) if(Lspeed > 100) Lspeed = 100; @@ -184,21 +184,20 @@ */ void pl_buzzer(void const *args) { - while(true) - { - stdio_mutex.lock(); + while(true) { + stdio_mutex.lock(); float pulse_delay = 150+((float)freq*10); pulse_delay= 1000/pulse_delay; stdio_mutex.unlock(); - // bt.lock(); + // bt.lock(); //bt.printf("s;%lf;s\n\r",pulse_delay); //bt.unlock(); buzzer=1; //Thread::wait(pulse_delay); buzzer=0; //Thread::wait(pulse_delay); - } - + } + //freq = 150+(freq*10); //buzzer.period_us(1000000/freq); // 4 second period //buzz.pulsewidth(2); // 2 second pulse (on) @@ -206,51 +205,47 @@ } void Imu_yaw(void const *args) { -while(true) + while(true) -{ - accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); - int m_seconds = r_time(); + { + 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; + 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; + 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); -} + Thread:: wait(50); + } } 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); -} + 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(); + Thread:: wait(50); + } } \ No newline at end of file