First import from throwbot
Revision 0:b74b6d70347d, committed 2014-10-05
- Comitter:
- Throwbot
- Date:
- Sun Oct 05 12:21:03 2014 +0000
- Commit message:
- Ebot Robot Code
Changed in this revision
robot.cpp | Show annotated file Show diff for this revision Revisions of this file |
robot.h | Show annotated file Show diff for this revision Revisions of this file |
--- /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