First import from throwbot
robot.cpp@0:b74b6d70347d, 2014-10-05 (annotated)
- Committer:
- Throwbot
- Date:
- Sun Oct 05 12:21:03 2014 +0000
- Revision:
- 0:b74b6d70347d
Ebot Robot Code
Who changed what in which revision?
User | Revision | Line number | New 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 | } |