Initial Commit
Embed:
(wiki syntax)
Show/hide line numbers
robot.cpp
00001 /* mbed ROBOT Library, for SUTD evobot project, Generation 1 00002 * Copyright (c) 2013, SUTD 00003 * Author: Mark VanderMeulen 00004 * Modified: Mayuran Saravanapavanantham (this code used for STARS) 00005 * 00006 * April 22, 2013 00007 */ 00008 00009 #include "robot.h" 00010 #include "math.h" 00011 //*********************************CONSTRUCTOR*********************************// 00012 //*********************************CONSTRUCTOR*********************************// 00013 HC05 bt(tx_bt,rx_bt,EN_BT); 00014 //QEI wheel (PTA16, PTA17, NC, 24); 00015 QEI right (PTA16, PTA17, NC, 300, QEI::X4_ENCODING); 00016 QEI left (PTA14, PTA13, NC, 300, QEI::X4_ENCODING); 00017 //Serial bt(rx_bt,tx_bt); 00018 //MPU6050 mpu(PTE0, PTE1); 00019 DigitalOut myled(myledd); 00020 DigitalOut key(PTA15); 00021 DigitalOut btSwitch(EN_BT); 00022 //AnalogIn currentSensor(CURRENTSENSOR_PIN); 00023 DigitalOut buzzer(buzz); 00024 00025 AnalogIn LDRsensor1(LDR1); 00026 AnalogIn LDRsensor2(LDR2); 00027 //AnalogIn voltageSensor(VOLTAGESENSOR_PIN); 00028 //PwmOut buzzer(buzz); 00029 PwmOut PWMA(MOT_PWMA_PIN); 00030 PwmOut PWMB(MOT_PWMB_PIN); 00031 DigitalOut AIN1(MOT_AIN1_PIN); 00032 DigitalOut AIN2(MOT_AIN2_PIN); 00033 DigitalOut BIN1(MOT_BIN1_PIN); 00034 DigitalOut BIN2(MOT_BIN2_PIN); 00035 DigitalOut STBY(MOT_STBY_PIN); 00036 00037 DigitalOut SRX(PTB10); 00038 TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN); 00039 TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN); 00040 Motors motors( &MotorA, &MotorB, MOT_STBY_PIN); 00041 00042 AnalogIn uL(ulL); 00043 AnalogIn uF(ulF); 00044 AnalogIn uR(ulR); 00045 AnalogIn urR(ulrR); 00046 AnalogIn urL(ulrL); 00047 AnalogIn uB(ulB); 00048 MPU6050 accelgyro; 00049 int16_t ax, ay, az; 00050 int16_t gx, gy, gz; 00051 int rMotor = 1; 00052 int lMotor = 1; 00053 int m_speed = 100; 00054 int speed; 00055 Mutex stdio_mutex; 00056 int freq=0; 00057 Timer t; 00058 int heading=0; 00059 int last_time=0; 00060 int dy =0; 00061 int dx=0; 00062 float left_current_reading=0; 00063 float right_current_reading= 0; 00064 float left_change = 0; 00065 float right_change =0; 00066 float left_prev_read=0; 00067 float right_prev_read=0; 00068 int delta_y=0; 00069 int delta_x=0; 00070 float delta_thetha=0; 00071 float encoder_yaw =0; 00072 float G_thetha=0; 00073 int Encoder_x=0; 00074 int Encoder_y=0; 00075 int dtheta=0; 00076 int software_interuupt; 00077 int Rmotor_speed=0; 00078 int Lmotor_speed=0; 00079 char Selected_robot; 00080 bool bt_connected=false; 00081 int r_time () 00082 { 00083 int mseconds = (int)time(NULL)+(int)t.read_ms(); 00084 return mseconds; 00085 } 00086 void initRobot() 00087 { 00088 key = 0; 00089 //btSwitch = 1; 00090 bt.start(); 00091 myled = 0; 00092 wait_ms(500); 00093 bt.baud(BaudRate_bt); 00094 accelgyro.initialize(); 00095 MotorA.scale = R_MOTOR_SCALE; 00096 MotorB.scale = L_MOTOR_SCALE; 00097 t.start(); 00098 SRX = 1; 00099 wait_us(30); 00100 SRX=0; 00101 wait_ms(300); 00102 SRX = 1; 00103 wait_us(30); 00104 SRX=0; 00105 wait(1); 00106 myled = 1; 00107 } 00108 00109 //*********************************MOTORS*********************************// 00110 void motor_control(int Lspeed, int Rspeed) 00111 { 00112 //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed 00113 if (!Lspeed && !Rspeed) { //stop// 00114 STBY = 0; 00115 } else 00116 STBY = 1; 00117 //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10) 00118 if(Lspeed > 100) Lspeed = 100; 00119 else if (Lspeed < -100) Lspeed = -100; 00120 else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15 00121 else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15; 00122 if(Rspeed > 100) Rspeed = 100; 00123 else if (Rspeed < -100) Rspeed = -100; 00124 else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15; 00125 else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15; 00126 if (!Rspeed) { //if right motor is stopped 00127 AIN1 = 0; 00128 AIN2 = 0; 00129 PWMA = 0; 00130 } else if (!Lspeed) { //if left motor is stopped 00131 BIN1 = 0; 00132 BIN2 = 0; 00133 PWMB = 0; 00134 } 00135 //RIGHT MOTOR// 00136 if(Rspeed > 0) { //Right motor fwd 00137 AIN1 = MOTOR_R_DIRECTION; //set the motor direction 00138 AIN2 = !AIN1; 00139 } else { //Right motor reverse 00140 AIN2 = MOTOR_R_DIRECTION; 00141 AIN1 = !AIN2; 00142 } 00143 PWMA = abs(Rspeed/100.0); 00144 //LEFT MOTOR// 00145 if(Lspeed >0) { 00146 BIN1 = MOTOR_L_DIRECTION; 00147 BIN2 = !BIN1; 00148 } else { 00149 BIN2 = MOTOR_L_DIRECTION; 00150 BIN1 = !BIN2; 00151 } 00152 PWMB = abs(Lspeed/100.0); 00153 } 00154 00155 void stop() 00156 { 00157 motor_control(0,0); 00158 } 00159 void set_speed(int Speed) 00160 { 00161 speed = Speed; 00162 motor_control(speed,speed); 00163 } 00164 00165 double ldrread1() 00166 { 00167 double r = LDRsensor1.read(); 00168 return r; 00169 00170 } 00171 double ldrread2() 00172 { 00173 double r = LDRsensor2.read(); 00174 return r; 00175 00176 } 00177 void Led_on() 00178 { 00179 // pulseIn 00180 myled= 0; 00181 } 00182 void Led_off() 00183 { 00184 // pulseIn 00185 myled= 1; 00186 } 00187 /*double pl_buzzer() 00188 { 00189 for (int i=0;i<1000;i++) 00190 { 00191 int freq = 150+(i*10); 00192 buzzer=1; 00193 wait_us(1000000/freq); 00194 buzzer=0; 00195 wait_us(1000000/freq); 00196 wait_ms(1); 00197 } 00198 00199 } 00200 */ 00201 void pl_buzzer(int freq, int f_time) 00202 { 00203 int elapsed_time=0; 00204 while (elapsed_time < f_time*10) { 00205 buzzer=1; 00206 wait_us(1000000/freq); 00207 buzzer=0; 00208 wait_us(1000000/freq); 00209 elapsed_time++; 00210 } 00211 00212 } 00213 void Imu_yaw(void const *args) 00214 { 00215 while(true) 00216 00217 { 00218 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 00219 int m_seconds = r_time(); 00220 float dt = ((float)(m_seconds-last_time))/1000; 00221 last_time=m_seconds; 00222 if ((gz)<800&& gz>-800) { 00223 gz=0; 00224 } 00225 stdio_mutex.lock(); 00226 heading = heading + (dt*gz)*3/4/100; 00227 if(heading>360) 00228 heading= heading -360; 00229 else if (heading <0) 00230 heading = heading +360; 00231 stdio_mutex.unlock(); 00232 Thread:: wait(50); 00233 } 00234 } 00235 void encoder_thread(void const *args) 00236 { 00237 while(true) { 00238 left_current_reading=left.getPulses()*(-1)/9.85; 00239 right_current_reading= right.getPulses()/10; 00240 left_change = left_current_reading- left_prev_read; 00241 right_change =right_current_reading- right_prev_read; 00242 left_prev_read=left_current_reading; 00243 right_prev_read=right_current_reading; 00244 delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2; 00245 delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2; 00246 delta_thetha=atan((right_change-left_change)/100); 00247 encoder_yaw =encoder_yaw+delta_thetha; 00248 G_thetha=encoder_yaw*180/M_PI; //global thetha, overall 00249 Encoder_x=Encoder_x+delta_x; 00250 Encoder_y=Encoder_y+delta_y; 00251 stdio_mutex.lock(); 00252 dx=delta_x+dx; 00253 dy=delta_y+dy; 00254 dtheta=delta_thetha*180/M_PI; 00255 //bt.lock(); 00256 //bt.printf(">>D;%0.2lf;%0.2lf;%0.2lf;%0.2lf;%d;%d;%d;\r\n", 00257 // left_current_reading,right_current_reading,left_change ,right_change,\ 00258 // dx,dy,\ 00259 // dtheta); 00260 //bt.unlock(); 00261 stdio_mutex.unlock(); 00262 Thread:: wait(50); 00263 } 00264 }
Generated on Mon Jul 18 2022 14:52:03 by 1.7.2