Initial Commit
robot.cpp
- Committer:
- Throwbot
- Date:
- 2014-10-21
- Revision:
- 5:a95a6243c118
- Parent:
- 4:0eeea5f05e28
File content as of revision 5:a95a6243c118:
/* 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 right (PTA16, PTA17, NC, 300, QEI::X4_ENCODING); QEI left (PTA14, PTA13, NC, 300, 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); 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); AnalogIn uR(ulR); AnalogIn urR(ulrR); AnalogIn urL(ulrL); AnalogIn uB(ulB); MPU6050 accelgyro; int16_t ax, ay, az; int16_t gx, gy, gz; int rMotor = 1; int lMotor = 1; int m_speed = 100; int speed; Mutex stdio_mutex; int freq=0; Timer t; int heading=0; int last_time=0; int dy =0; int dx=0; float left_current_reading=0; float right_current_reading= 0; float left_change = 0; float right_change =0; float left_prev_read=0; float right_prev_read=0; int delta_y=0; int delta_x=0; float delta_thetha=0; float encoder_yaw =0; float G_thetha=0; int Encoder_x=0; int Encoder_y=0; int dtheta=0; 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(); return mseconds; } void initRobot() { key = 0; //btSwitch = 1; bt.start(); myled = 0; 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); SRX=0; wait_ms(300); SRX = 1; wait_us(30); SRX=0; wait(1); 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(int freq, int f_time) { int elapsed_time=0; while (elapsed_time < f_time*10) { buzzer=1; wait_us(1000000/freq); buzzer=0; wait_us(1000000/freq); elapsed_time++; } } void Imu_yaw(void const *args) { while(true) { 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; 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; stdio_mutex.unlock(); Thread:: wait(50); } } void encoder_thread(void const *args) { while(true) { left_current_reading=left.getPulses()*(-1)/9.85; right_current_reading= right.getPulses()/10; 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; //bt.lock(); //bt.printf(">>D;%0.2lf;%0.2lf;%0.2lf;%0.2lf;%d;%d;%d;\r\n", // left_current_reading,right_current_reading,left_change ,right_change,\ // dx,dy,\ // dtheta); //bt.unlock(); stdio_mutex.unlock(); Thread:: wait(50); } }