EdgeBotix / Robot

Initial Commit

robot.cpp@0:b74b6d70347d, 2014-10-05 (annotated)

Committer:
Throwbot
Date:
Sun Oct 05 12:21:03 2014 +0000
Revision:
0:b74b6d70347d
Child:
1:282467cbebb3
Ebot Robot Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 0:b74b6d70347d 1 /* mbed ROBOT Library, for SUTD evobot project, Generation 1
Throwbot 0:b74b6d70347d 2 * Copyright (c) 2013, SUTD
Throwbot 0:b74b6d70347d 3 * Author: Mark VanderMeulen
Throwbot 0:b74b6d70347d 4 * Modified: Mayuran Saravanapavanantham (this code used for STARS)
Throwbot 0:b74b6d70347d 5 *
Throwbot 0:b74b6d70347d 6 * April 22, 2013
Throwbot 0:b74b6d70347d 7 */
Throwbot 0:b74b6d70347d 8
Throwbot 0:b74b6d70347d 9 #include "robot.h"
Throwbot 0:b74b6d70347d 10 #include "math.h"
Throwbot 0:b74b6d70347d 11 //*********************************CONSTRUCTOR*********************************//
Throwbot 0:b74b6d70347d 12 //*********************************CONSTRUCTOR*********************************//
Throwbot 0:b74b6d70347d 13 HC05 bt(tx_bt,rx_bt,EN_BT);
Throwbot 0:b74b6d70347d 14 //QEI wheel (PTA16, PTA17, NC, 24);
Throwbot 0:b74b6d70347d 15 QEI left (PTA16, PTA17, NC, 150, QEI::X4_ENCODING);
Throwbot 0:b74b6d70347d 16 QEI right (PTA14, PTA13, NC, 150, QEI::X4_ENCODING);
Throwbot 0:b74b6d70347d 17 //Serial bt(rx_bt,tx_bt);
Throwbot 0:b74b6d70347d 18 //MPU6050 mpu(PTE0, PTE1);
Throwbot 0:b74b6d70347d 19 DigitalOut myled(myledd);
Throwbot 0:b74b6d70347d 20 DigitalOut key(PTA15);
Throwbot 0:b74b6d70347d 21 DigitalOut btSwitch(EN_BT);
Throwbot 0:b74b6d70347d 22 //AnalogIn currentSensor(CURRENTSENSOR_PIN);
Throwbot 0:b74b6d70347d 23 DigitalOut buzzer(buzz);
Throwbot 0:b74b6d70347d 24
Throwbot 0:b74b6d70347d 25 AnalogIn LDRsensor1(LDR1);
Throwbot 0:b74b6d70347d 26 AnalogIn LDRsensor2(LDR2);
Throwbot 0:b74b6d70347d 27 //AnalogIn voltageSensor(VOLTAGESENSOR_PIN);
Throwbot 0:b74b6d70347d 28 //PwmOut buzzer(buzz);
Throwbot 0:b74b6d70347d 29 PwmOut PWMA(MOT_PWMA_PIN);
Throwbot 0:b74b6d70347d 30 PwmOut PWMB(MOT_PWMB_PIN);
Throwbot 0:b74b6d70347d 31 DigitalOut AIN1(MOT_AIN1_PIN);
Throwbot 0:b74b6d70347d 32 DigitalOut AIN2(MOT_AIN2_PIN);
Throwbot 0:b74b6d70347d 33 DigitalOut BIN1(MOT_BIN1_PIN);
Throwbot 0:b74b6d70347d 34 DigitalOut BIN2(MOT_BIN2_PIN);
Throwbot 0:b74b6d70347d 35 DigitalOut STBY(MOT_STBY_PIN);
Throwbot 0:b74b6d70347d 36 int rMotor = -1;
Throwbot 0:b74b6d70347d 37 int lMotor = -1;
Throwbot 0:b74b6d70347d 38 int m_speed = 100;
Throwbot 0:b74b6d70347d 39 int speed;
Throwbot 0:b74b6d70347d 40 Mutex stdio_mutex;
Throwbot 0:b74b6d70347d 41 int freq=0;
Throwbot 0:b74b6d70347d 42 /*
Throwbot 0:b74b6d70347d 43 double target_angle=0;
Throwbot 0:b74b6d70347d 44 double rz; //Direction robot is facing
Throwbot 0:b74b6d70347d 45 double Irz; //integral of the rotation offset from target. (Optionally) Used for PID control of direction
Throwbot 0:b74b6d70347d 46 double angle_origin; //Angle of origin (can be changed later, or set if robot starts at known angle)
Throwbot 0:b74b6d70347d 47 bool AUTO_ORIENT; //if this flag is 1, the robot automatically orients itself to selected direction
Throwbot 0:b74b6d70347d 48 bool REMOTE_CONTROL; //if this flag is 1, the robot will be controlled over bluetooth
Throwbot 0:b74b6d70347d 49 int acc[3];
Throwbot 0:b74b6d70347d 50 int gyr[3];
Throwbot 0:b74b6d70347d 51 bool MPU_OK;
Throwbot 0:b74b6d70347d 52 double timeNext;
Throwbot 0:b74b6d70347d 53 int speed;
Throwbot 0:b74b6d70347d 54 int accdata[3]; //data from accelerometer (raw)
Throwbot 0:b74b6d70347d 55 int gyrodata[3]; //data from gyro (raw)
Throwbot 0:b74b6d70347d 56 //double gyroCorrect; //= 3720; //divide by this to get gyro data in RAD/SECOND. This is above as a #DEFINE
Throwbot 0:b74b6d70347d 57 int gyroOffset[3]; //Correction value for each gyroscope to zero the values.
Throwbot 0:b74b6d70347d 58 int accOffset[3]; //correction value for each accelerometer
Throwbot 0:b74b6d70347d 59 double dx = 0; //The current displacement in the x-axis (side-side)
Throwbot 0:b74b6d70347d 60 double dy = 0; //The current displacement in the y-axis (forward-back)
Throwbot 0:b74b6d70347d 61 double dz = 0; //The current displacement in the z-axis (up-down)
Throwbot 0:b74b6d70347d 62 double origin = 0;
Throwbot 0:b74b6d70347d 63 int freq=0;
Throwbot 0:b74b6d70347d 64 */
Throwbot 0:b74b6d70347d 65 void initRobot()
Throwbot 0:b74b6d70347d 66 {
Throwbot 0:b74b6d70347d 67
Throwbot 0:b74b6d70347d 68 key = 0;
Throwbot 0:b74b6d70347d 69 //btSwitch = 1;
Throwbot 0:b74b6d70347d 70 bt.start();
Throwbot 0:b74b6d70347d 71 myled = 0;
Throwbot 0:b74b6d70347d 72 wait_ms(500);
Throwbot 0:b74b6d70347d 73 bt.baud(BaudRate_bt);
Throwbot 0:b74b6d70347d 74 myled = 1;
Throwbot 0:b74b6d70347d 75 }
Throwbot 0:b74b6d70347d 76
Throwbot 0:b74b6d70347d 77 //*********************************MOTORS*********************************//
Throwbot 0:b74b6d70347d 78 void motor_control(int Lspeed, int Rspeed)
Throwbot 0:b74b6d70347d 79 {
Throwbot 0:b74b6d70347d 80 //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
Throwbot 0:b74b6d70347d 81 if (!Lspeed && !Rspeed) //stop//
Throwbot 0:b74b6d70347d 82 { STBY = 0;
Throwbot 0:b74b6d70347d 83 }
Throwbot 0:b74b6d70347d 84 else
Throwbot 0:b74b6d70347d 85 STBY = 1;
Throwbot 0:b74b6d70347d 86 //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
Throwbot 0:b74b6d70347d 87 if(Lspeed > 100) Lspeed = 100;
Throwbot 0:b74b6d70347d 88 else if (Lspeed < -100) Lspeed = -100;
Throwbot 0:b74b6d70347d 89 else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15
Throwbot 0:b74b6d70347d 90 else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15;
Throwbot 0:b74b6d70347d 91 if(Rspeed > 100) Rspeed = 100;
Throwbot 0:b74b6d70347d 92 else if (Rspeed < -100) Rspeed = -100;
Throwbot 0:b74b6d70347d 93 else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15;
Throwbot 0:b74b6d70347d 94 else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15;
Throwbot 0:b74b6d70347d 95 if (!Rspeed) { //if right motor is stopped
Throwbot 0:b74b6d70347d 96 AIN1 = 0;
Throwbot 0:b74b6d70347d 97 AIN2 = 0;
Throwbot 0:b74b6d70347d 98 PWMA = 0;
Throwbot 0:b74b6d70347d 99 } else if (!Lspeed) { //if left motor is stopped
Throwbot 0:b74b6d70347d 100 BIN1 = 0;
Throwbot 0:b74b6d70347d 101 BIN2 = 0;
Throwbot 0:b74b6d70347d 102 PWMB = 0;
Throwbot 0:b74b6d70347d 103 }
Throwbot 0:b74b6d70347d 104 //RIGHT MOTOR//
Throwbot 0:b74b6d70347d 105 if(Rspeed > 0) { //Right motor fwd
Throwbot 0:b74b6d70347d 106 AIN1 = MOTOR_R_DIRECTION; //set the motor direction
Throwbot 0:b74b6d70347d 107 AIN2 = !AIN1;
Throwbot 0:b74b6d70347d 108 } else { //Right motor reverse
Throwbot 0:b74b6d70347d 109 AIN2 = MOTOR_R_DIRECTION;
Throwbot 0:b74b6d70347d 110 AIN1 = !AIN2;
Throwbot 0:b74b6d70347d 111 }
Throwbot 0:b74b6d70347d 112 PWMA = abs(Rspeed/100.0);
Throwbot 0:b74b6d70347d 113 //LEFT MOTOR//
Throwbot 0:b74b6d70347d 114 if(Lspeed >0) {
Throwbot 0:b74b6d70347d 115 BIN1 = MOTOR_L_DIRECTION;
Throwbot 0:b74b6d70347d 116 BIN2 = !BIN1;
Throwbot 0:b74b6d70347d 117 } else {
Throwbot 0:b74b6d70347d 118 BIN2 = MOTOR_L_DIRECTION;
Throwbot 0:b74b6d70347d 119 BIN1 = !BIN2;
Throwbot 0:b74b6d70347d 120 }
Throwbot 0:b74b6d70347d 121 PWMB = abs(Lspeed/100.0);
Throwbot 0:b74b6d70347d 122 }
Throwbot 0:b74b6d70347d 123
Throwbot 0:b74b6d70347d 124 void stop()
Throwbot 0:b74b6d70347d 125 {
Throwbot 0:b74b6d70347d 126 motor_control(0,0);
Throwbot 0:b74b6d70347d 127 }
Throwbot 0:b74b6d70347d 128 void set_speed(int Speed)
Throwbot 0:b74b6d70347d 129 {
Throwbot 0:b74b6d70347d 130 speed = Speed;
Throwbot 0:b74b6d70347d 131 motor_control(speed,speed);
Throwbot 0:b74b6d70347d 132 }
Throwbot 0:b74b6d70347d 133
Throwbot 0:b74b6d70347d 134 double ldrread1()
Throwbot 0:b74b6d70347d 135 {
Throwbot 0:b74b6d70347d 136 double r = LDRsensor1.read();
Throwbot 0:b74b6d70347d 137 return r;
Throwbot 0:b74b6d70347d 138
Throwbot 0:b74b6d70347d 139 }
Throwbot 0:b74b6d70347d 140 double ldrread2()
Throwbot 0:b74b6d70347d 141 {
Throwbot 0:b74b6d70347d 142 double r = LDRsensor2.read();
Throwbot 0:b74b6d70347d 143 return r;
Throwbot 0:b74b6d70347d 144
Throwbot 0:b74b6d70347d 145 }
Throwbot 0:b74b6d70347d 146 void Led_on()
Throwbot 0:b74b6d70347d 147 {
Throwbot 0:b74b6d70347d 148 // pulseIn
Throwbot 0:b74b6d70347d 149 myled= 0;
Throwbot 0:b74b6d70347d 150 }
Throwbot 0:b74b6d70347d 151 void Led_off()
Throwbot 0:b74b6d70347d 152 {
Throwbot 0:b74b6d70347d 153 // pulseIn
Throwbot 0:b74b6d70347d 154 myled= 1;
Throwbot 0:b74b6d70347d 155 }
Throwbot 0:b74b6d70347d 156 /*double pl_buzzer()
Throwbot 0:b74b6d70347d 157 {
Throwbot 0:b74b6d70347d 158 for (int i=0;i<1000;i++)
Throwbot 0:b74b6d70347d 159 {
Throwbot 0:b74b6d70347d 160 int freq = 150+(i*10);
Throwbot 0:b74b6d70347d 161 buzzer=1;
Throwbot 0:b74b6d70347d 162 wait_us(1000000/freq);
Throwbot 0:b74b6d70347d 163 buzzer=0;
Throwbot 0:b74b6d70347d 164 wait_us(1000000/freq);
Throwbot 0:b74b6d70347d 165 wait_ms(1);
Throwbot 0:b74b6d70347d 166 }
Throwbot 0:b74b6d70347d 167
Throwbot 0:b74b6d70347d 168 }
Throwbot 0:b74b6d70347d 169 */
Throwbot 0:b74b6d70347d 170 void pl_buzzer(void const *args)
Throwbot 0:b74b6d70347d 171 {
Throwbot 0:b74b6d70347d 172 while(true)
Throwbot 0:b74b6d70347d 173 {
Throwbot 0:b74b6d70347d 174 stdio_mutex.lock();
Throwbot 0:b74b6d70347d 175 float pulse_delay = 150+((float)freq*10);
Throwbot 0:b74b6d70347d 176 pulse_delay= 1000/pulse_delay;
Throwbot 0:b74b6d70347d 177 stdio_mutex.unlock();
Throwbot 0:b74b6d70347d 178 // bt.lock();
Throwbot 0:b74b6d70347d 179 //bt.printf("s;%lf;s\n\r",pulse_delay);
Throwbot 0:b74b6d70347d 180 //bt.unlock();
Throwbot 0:b74b6d70347d 181 buzzer=1;
Throwbot 0:b74b6d70347d 182 Thread::wait(pulse_delay);
Throwbot 0:b74b6d70347d 183 buzzer=0;
Throwbot 0:b74b6d70347d 184 Thread::wait(pulse_delay);
Throwbot 0:b74b6d70347d 185 }
Throwbot 0:b74b6d70347d 186
Throwbot 0:b74b6d70347d 187 //freq = 150+(freq*10);
Throwbot 0:b74b6d70347d 188 //buzzer.period_us(1000000/freq); // 4 second period
Throwbot 0:b74b6d70347d 189 //buzz.pulsewidth(2); // 2 second pulse (on)
Throwbot 0:b74b6d70347d 190 //buzzer.write(0.5f); // 50% duty cycle
Throwbot 0:b74b6d70347d 191 }