Diff: robot.cpp

Revision:

4:0eeea5f05e28

Parent:

3:3e3314102e56

Child:

5:a95a6243c118

--- a/robot.cpp	Sun Oct 12 23:27:33 2014 +0000
+++ b/robot.cpp	Tue Oct 14 17:54:20 2014 +0000
@@ -35,6 +35,9 @@
 DigitalOut STBY(MOT_STBY_PIN);
 
 DigitalOut SRX(PTB10);
+TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN);
+TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN);
+Motors motors( &MotorA, &MotorB, MOT_STBY_PIN);
 
 AnalogIn uL(ulL);
 AnalogIn uF(ulF);
@@ -73,6 +76,8 @@
 int software_interuupt;
 int Rmotor_speed=0;
 int Lmotor_speed=0;
+char Selected_robot;
+bool bt_connected=false;
 int r_time ()
 {
     int mseconds = (int)time(NULL)+(int)t.read_ms();
@@ -87,6 +92,8 @@
     wait_ms(500);
     bt.baud(BaudRate_bt);
     accelgyro.initialize();
+    MotorA.scale = R_MOTOR_SCALE;
+    MotorB.scale = L_MOTOR_SCALE;
     t.start();
     SRX = 1;
     wait_us(30);
@@ -191,26 +198,17 @@
 
 }
 */
-void pl_buzzer(void const *args)
+void pl_buzzer(int freq, int f_time)
 {
-    while(true) {
-        stdio_mutex.lock();
-        float pulse_delay = 150+((float)freq*10);
-        pulse_delay= 1000/pulse_delay;
-        stdio_mutex.unlock();
-        // bt.lock();
-        //bt.printf("s;%lf;s\n\r",pulse_delay);
-        //bt.unlock();
+       int elapsed_time=0;
+       while (elapsed_time < f_time*10) {
         buzzer=1;
-        //Thread::wait(pulse_delay);
+        wait_us(1000000/freq);
         buzzer=0;
-        //Thread::wait(pulse_delay);
-    }
+        wait_us(1000000/freq);
+        elapsed_time++;
+        }
 
-    //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)
 {