Revision 0:b74b6d70347d, committed 2014-10-05

Comitter:

Throwbot

Date:

Sun Oct 05 12:21:03 2014 +0000

Child:

1:282467cbebb3

Commit message:

Ebot Robot Code

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.h	Sun Oct 05 12:21:03 2014 +0000
@@ -0,0 +1,135 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+#include "QEI.h"
+#include "mbed.h"
+#include "rtos.h"
+#include "HC05.h"
+//#include "MPU6050.h"
+//#include "HC05.h"
+//#include "ACS712.h"
+//#include "MPU6050_6Axis_MotionApps20.h"
+//#include "DMP.h"
+//#include "IMUDATA.h"
+//#include "ADNS5090.h"
+#define BaudRate_bt 115200      //Baud rate of 9600
+#define tx_bt       PTA2        //Bluetooth connection pins
+#define rx_bt       PTA1        //Bluetooth connection pins
+#define EN_BT       PTA4        //Bluetooth connection pins
+//#define tx_mpu      PTE0    //MPU connection pins
+//#define rx_mpu      PTE1    //MPU connection pins
+//#define mpu_bandwidth MPU6050_BW_20 //set the MPU low pass filter bandwidth to 20hz
+
+#define myledd PTB8        //status LED pin
+#define CURRENTSENSOR_PIN PTB3
+#define VOLTAGESENSOR_PIN PTB0
+
+#define CURRENT_R1 180 //160.0     //values of the current sensor opamp resistors
+#define CURRENT_R2 10
+#define CURRENT_R3 80
+#define CURRENT_R4 84.7
+#define VREF3_3 3.3        //digital logic voltage
+#define VREF5 5.0       //5v voltage        //NOTE: 5v voltage is consistent when using new batts, but not using old blue batts
+
+
+//#define irL PTB1
+//#define irC PTB3
+//#define irR PTB2
+
+#define LDR1 PTB1
+#define LDR2 PTB2
+#define buzz PTC8
+
+#define MOT_PWMA_PIN   PTA5    //Motor control pins    
+#define MOT_PWMB_PIN   PTA12
+#define MOT_STBY_PIN   PTE31
+#define MOT_AIN1_PIN   PTE30
+#define MOT_AIN2_PIN   PTE29
+#define MOT_BIN1_PIN   PTE24
+#define MOT_BIN2_PIN   PTE25
+
+#define M_PI 3.14159265359  //PI
+#define gyroCorrect 3720    //divide raw gyro data by this to get result in RAD/SECOND (if gyroRange is 500 rad/s)
+
+//Correct direction of motors. If number = 1, forward. If number = 0, backwards (for if motors are wired backwards)
+#define MOTOR_R_DIRECTION   1
+#define MOTOR_L_DIRECTION   1
+
+#define MOTOR_INTERVAL 20     //defines the interval (in milliseconds) between when motor can be set
+//NOTE: Can't make this less than 20ms, because that is the PWM frequency
+
+//Key bindings for remote control robot - for the future try to use arrow keys instead of 'asdw'
+#define ctrl_forward    'i'         //forward
+#define ctrl_backward   'k'         //back
+#define ctrl_left       'j'         //turn left
+#define ctrl_right      'l'         //turn right
+#define ctrl_calibrate  'c'         //re-calibrate the accelerometer and gyro
+#define ctrl_turn_angle_cw    'o'         // turn angle
+#define ctrl_turn_angle_ccw    'p'
+// The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
+//  "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
+//  only handles 4 byte transfers in the ATMega code.
+#define TRANSFER_SIZE   4
+
+extern   HC05 bt;  //bluetooth connection
+//extern   Serial bt;
+//extern  QEI wheel;
+extern QEI left;
+extern QEI right;
+//extern    MPU6050 mpu;   //MPU connection
+extern    DigitalOut myled;   //(PTE3) Processor LED (1 = off, 0 = on)
+//extern    DigitalOut btSwitch;
+//extern    AnalogIn currentSensor;
+extern    DigitalOut buzzer;
+extern    DigitalOut key;
+//extern    PwmOut buzzer;
+extern    AnalogIn LDRsensor1;
+extern    AnalogIn LDRsensor2;
+//extern    AnalogIn voltageSensor;
+    ///////////   Motor control variables   ///////////
+extern    PwmOut PWMA;//(MOT_PWMA_PIN);
+extern    PwmOut PWMB;//(MOT_PWMB_PIN);
+extern    DigitalOut AIN1;//(MOT_AIN1_PIN);
+extern    DigitalOut AIN2;//(MOT_AIN2_PIN);
+extern    DigitalOut BIN1;//(MOT_BIN1_PIN);
+extern    DigitalOut BIN2;//(MOT_BIN2_PIN);
+extern    DigitalOut STBY;//(MOT_STBY_PIN);
+    void motor_control(int Lspeed, int Rspeed);  //Input speed for motors. Integer between 0 and 100
+    void stop();                           //stop motors
+    void set_direction(double angle);            //set angle for the robot to face (from origin)
+    void set_direction_deg(double angle);
+    void set_speed(int Speed );      //set speed for robot to travel at
+    void auto_enable(bool x);
+    /**
+    *   MPU CONTROLS
+    */
+    //  calibrate the gyro and accelerometer  //
+   void calibrate();
+    /**
+    *   Status: find the distance, orientation, battery values, etc of the robot
+    */
+    //void distanceTravelled(double x[3])
+    //void orientation(something quaternion? on xy plane?)
+    double getCurrent();   //Get the current drawn by the robot
+    double getCurrent(int n); //get the current, averaged over n samples
+    double getVoltage();    //get the battery voltage (ask connor for completed function)
+    void getIMU(float *adata, float *gdata);
+    double ldrread1();
+    double ldrread2();
+    //void pl_buzzer1();
+    void Led_on();
+    void Led_off();
+    double return_rotation();
+    int isMPU();
+    void initRobot();
+    extern double pl_buzzer();
+    extern void pl_buzzer(void const *args);
+    extern int freq; 
+    extern int rMotor;
+    extern int lMotor ;
+    extern int m_speed;
+    extern Mutex stdio_mutex; 
+    
+
+    //Wireless connections
+
+#endif
\ No newline at end of file