Diff: robot.cpp

Revision:

0:b74b6d70347d

diff -r 000000000000 -r b74b6d70347d robot.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Sun Oct 05 12:21:03 2014 +0000
@@ -0,0 +1,191 @@
+/* mbed ROBOT Library, for SUTD evobot project, Generation 1
+ * Copyright (c) 2013, SUTD
+ * Author: Mark VanderMeulen
+ * Modified: Mayuran Saravanapavanantham (this code used for STARS)
+ *
+ * April 22, 2013
+ */
+
+#include "robot.h"
+#include "math.h"
+//*********************************CONSTRUCTOR*********************************//
+//*********************************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);
+//Serial bt(rx_bt,tx_bt);
+//MPU6050 mpu(PTE0, PTE1);
+DigitalOut myled(myledd);
+DigitalOut key(PTA15);
+DigitalOut btSwitch(EN_BT);
+//AnalogIn  currentSensor(CURRENTSENSOR_PIN);
+DigitalOut buzzer(buzz);
+
+AnalogIn LDRsensor1(LDR1);
+AnalogIn LDRsensor2(LDR2);
+//AnalogIn voltageSensor(VOLTAGESENSOR_PIN);
+//PwmOut  buzzer(buzz);
+PwmOut PWMA(MOT_PWMA_PIN);
+PwmOut PWMB(MOT_PWMB_PIN);
+DigitalOut AIN1(MOT_AIN1_PIN);
+DigitalOut AIN2(MOT_AIN2_PIN);
+DigitalOut BIN1(MOT_BIN1_PIN);
+DigitalOut BIN2(MOT_BIN2_PIN);
+DigitalOut STBY(MOT_STBY_PIN);
+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;
+*/ 
+void initRobot()
+{
+
+    key  = 0;
+    //btSwitch = 1;
+    bt.start();
+    myled = 0;
+    wait_ms(500);
+    bt.baud(BaudRate_bt);
+    myled = 1;
+}
+
+//*********************************MOTORS*********************************//
+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
+        STBY = 1;
+    //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
+    if(Lspeed > 100) Lspeed = 100;
+    else if (Lspeed < -100) Lspeed = -100;
+    else if (Lspeed < 0 && Lspeed > -15)   Lspeed = -15;    //prevent speed from being less than 15
+    else if (Lspeed > 0 &&  Lspeed < 15)    Lspeed = 15;
+    if(Rspeed > 100) Rspeed = 100;
+    else if (Rspeed < -100) Rspeed = -100;
+    else if (Rspeed < 0 && Rspeed > -15)   Rspeed = -15;
+    else if (Rspeed > 0 &&  Rspeed < 15)    Rspeed = 15;
+    if (!Rspeed) {  //if right motor is stopped
+        AIN1 = 0;
+        AIN2 = 0;
+        PWMA = 0;
+    } else if (!Lspeed) { //if left motor is stopped
+        BIN1 = 0;
+        BIN2 = 0;
+        PWMB = 0;
+    }
+    //RIGHT MOTOR//
+    if(Rspeed > 0) {     //Right motor fwd
+        AIN1 = MOTOR_R_DIRECTION;   //set the motor direction
+        AIN2 = !AIN1;
+    } else {     //Right motor reverse
+        AIN2 = MOTOR_R_DIRECTION;
+        AIN1 = !AIN2;
+    }
+    PWMA = abs(Rspeed/100.0);
+    //LEFT MOTOR//
+    if(Lspeed >0) {
+        BIN1 = MOTOR_L_DIRECTION;
+        BIN2 = !BIN1;
+    } else {
+        BIN2 = MOTOR_L_DIRECTION;
+        BIN1 = !BIN2;
+    }
+    PWMB = abs(Lspeed/100.0);
+}
+
+void stop()
+{
+    motor_control(0,0);
+}
+void set_speed(int Speed)
+{
+    speed = Speed;
+    motor_control(speed,speed);
+}
+
+double ldrread1()
+{
+    double r = LDRsensor1.read();
+    return r;
+
+}
+double ldrread2()
+{
+    double r = LDRsensor2.read();
+    return r;
+
+}
+void Led_on()
+{
+    // pulseIn
+    myled= 0;
+}
+void Led_off()
+{
+    // pulseIn
+    myled= 1;
+}
+/*double pl_buzzer()
+{
+      for (int i=0;i<1000;i++)
+    {
+        int freq = 150+(i*10);
+        buzzer=1;
+        wait_us(1000000/freq);
+        buzzer=0;
+        wait_us(1000000/freq);
+        wait_ms(1);
+    }
+
+}
+*/
+void pl_buzzer(void const *args)
+{
+         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();
+        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)
+    //buzzer.write(0.5f);  // 50% duty cycle
+}
\ No newline at end of file