Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
main.cpp@0:f3bf6c7e2283, 2012-04-20 (annotated)
- Committer:
- narshu
- Date:
- Fri Apr 20 20:39:35 2012 +0000
- Revision:
- 0:f3bf6c7e2283
- Child:
- 1:bbabbd997d21
inverted sonar echo input at pin14 and pin15
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
narshu | 0:f3bf6c7e2283 | 1 | #include "mbed.h" |
narshu | 0:f3bf6c7e2283 | 2 | #include "rtos.h" |
narshu | 0:f3bf6c7e2283 | 3 | #include "TSH.h" |
narshu | 0:f3bf6c7e2283 | 4 | #include "Kalman.h" |
narshu | 0:f3bf6c7e2283 | 5 | #include "globals.h" |
narshu | 0:f3bf6c7e2283 | 6 | #include "motors.h" |
narshu | 0:f3bf6c7e2283 | 7 | #include "math.h" |
narshu | 0:f3bf6c7e2283 | 8 | #include "system.h" |
narshu | 0:f3bf6c7e2283 | 9 | |
narshu | 0:f3bf6c7e2283 | 10 | //#include <iostream> |
narshu | 0:f3bf6c7e2283 | 11 | |
narshu | 0:f3bf6c7e2283 | 12 | //Interface declaration |
narshu | 0:f3bf6c7e2283 | 13 | //I2C i2c(p28, p27); // sda, scl |
narshu | 0:f3bf6c7e2283 | 14 | TSI2C i2c(p28, p27); |
narshu | 0:f3bf6c7e2283 | 15 | Serial pc(USBTX, USBRX); // tx, rx |
narshu | 0:f3bf6c7e2283 | 16 | Serial IRturret(p13, p14); |
narshu | 0:f3bf6c7e2283 | 17 | |
narshu | 0:f3bf6c7e2283 | 18 | DigitalOut OLED1(LED1); |
narshu | 0:f3bf6c7e2283 | 19 | DigitalOut OLED2(LED2); |
narshu | 0:f3bf6c7e2283 | 20 | DigitalOut OLED3(LED3); |
narshu | 0:f3bf6c7e2283 | 21 | DigitalOut OLED4(LED4); |
narshu | 0:f3bf6c7e2283 | 22 | |
narshu | 0:f3bf6c7e2283 | 23 | int16_t face; |
narshu | 0:f3bf6c7e2283 | 24 | |
narshu | 0:f3bf6c7e2283 | 25 | |
narshu | 0:f3bf6c7e2283 | 26 | Motors motors; |
narshu | 0:f3bf6c7e2283 | 27 | Kalman kalman(motors); |
narshu | 0:f3bf6c7e2283 | 28 | |
narshu | 0:f3bf6c7e2283 | 29 | float targetX = 1000, targetY = 1000, targetTheta = 0; |
narshu | 0:f3bf6c7e2283 | 30 | |
narshu | 0:f3bf6c7e2283 | 31 | // bytes packing for IR turret serial comm |
narshu | 0:f3bf6c7e2283 | 32 | union IRValue_t { |
narshu | 0:f3bf6c7e2283 | 33 | float IR_floats[6]; |
narshu | 0:f3bf6c7e2283 | 34 | int IR_ints[6]; |
narshu | 0:f3bf6c7e2283 | 35 | unsigned char IR_chars[24]; |
narshu | 0:f3bf6c7e2283 | 36 | } IRValues; |
narshu | 0:f3bf6c7e2283 | 37 | |
narshu | 0:f3bf6c7e2283 | 38 | //TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus) |
narshu | 0:f3bf6c7e2283 | 39 | //NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos! |
narshu | 0:f3bf6c7e2283 | 40 | |
narshu | 0:f3bf6c7e2283 | 41 | void vMotorThread(void const *argument); |
narshu | 0:f3bf6c7e2283 | 42 | void vPrintState(void const *argument); |
narshu | 0:f3bf6c7e2283 | 43 | void ai_thread (void const *argument); |
narshu | 0:f3bf6c7e2283 | 44 | void motion_thread(void const *argument); |
narshu | 0:f3bf6c7e2283 | 45 | void vIRValueISR (void); |
narshu | 0:f3bf6c7e2283 | 46 | void motorSpeedThread(void const *argument); |
narshu | 0:f3bf6c7e2283 | 47 | |
narshu | 0:f3bf6c7e2283 | 48 | float getAngle (float x, float y); |
narshu | 0:f3bf6c7e2283 | 49 | void getIRValue(void const *argument); |
narshu | 0:f3bf6c7e2283 | 50 | |
narshu | 0:f3bf6c7e2283 | 51 | |
narshu | 0:f3bf6c7e2283 | 52 | //Thread thr_AI(ai_thread); |
narshu | 0:f3bf6c7e2283 | 53 | //Thread thr_motion(motion_thread); |
narshu | 0:f3bf6c7e2283 | 54 | //Thread thr_getIRValue(getIRValue); |
narshu | 0:f3bf6c7e2283 | 55 | |
narshu | 0:f3bf6c7e2283 | 56 | Mutex targetlock; |
narshu | 0:f3bf6c7e2283 | 57 | bool flag_terminate = false; |
narshu | 0:f3bf6c7e2283 | 58 | |
narshu | 0:f3bf6c7e2283 | 59 | |
narshu | 0:f3bf6c7e2283 | 60 | float temp = 0; |
narshu | 0:f3bf6c7e2283 | 61 | |
narshu | 0:f3bf6c7e2283 | 62 | //Main loop |
narshu | 0:f3bf6c7e2283 | 63 | int main() { |
narshu | 0:f3bf6c7e2283 | 64 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 65 | pc.baud(115200); |
narshu | 0:f3bf6c7e2283 | 66 | IRturret.baud(9600); |
narshu | 0:f3bf6c7e2283 | 67 | IRturret.format(8,Serial::Odd,1); |
narshu | 0:f3bf6c7e2283 | 68 | //Thread thread(motorSpeedThread, NULL, osPriorityAboveNormal); //PID controller task. run at 50ms |
narshu | 0:f3bf6c7e2283 | 69 | //IRturret.attach(&vIRValueISR); |
narshu | 0:f3bf6c7e2283 | 70 | //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); |
narshu | 0:f3bf6c7e2283 | 71 | Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); |
narshu | 0:f3bf6c7e2283 | 72 | |
narshu | 0:f3bf6c7e2283 | 73 | pc.printf("We got to main! ;D\r\n"); |
narshu | 0:f3bf6c7e2283 | 74 | |
narshu | 0:f3bf6c7e2283 | 75 | //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! |
narshu | 0:f3bf6c7e2283 | 76 | while (1) { |
narshu | 0:f3bf6c7e2283 | 77 | // do nothing |
narshu | 0:f3bf6c7e2283 | 78 | Thread::wait(osWaitForever); |
narshu | 0:f3bf6c7e2283 | 79 | } |
narshu | 0:f3bf6c7e2283 | 80 | } |
narshu | 0:f3bf6c7e2283 | 81 | |
narshu | 0:f3bf6c7e2283 | 82 | void motorSpeedThread(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 83 | while (1) { |
narshu | 0:f3bf6c7e2283 | 84 | motors.speedRegulatorTask(); |
narshu | 0:f3bf6c7e2283 | 85 | Thread::wait(10); |
narshu | 0:f3bf6c7e2283 | 86 | } |
narshu | 0:f3bf6c7e2283 | 87 | } |
narshu | 0:f3bf6c7e2283 | 88 | |
narshu | 0:f3bf6c7e2283 | 89 | |
narshu | 0:f3bf6c7e2283 | 90 | void vMotorThread(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 91 | motors.resetEncoders(); |
narshu | 0:f3bf6c7e2283 | 92 | while (1) { |
narshu | 0:f3bf6c7e2283 | 93 | motors.setSpeed(20,-20); |
narshu | 0:f3bf6c7e2283 | 94 | Thread::wait(2000); |
narshu | 0:f3bf6c7e2283 | 95 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 96 | Thread::wait(5000); |
narshu | 0:f3bf6c7e2283 | 97 | motors.setSpeed(-20,20); |
narshu | 0:f3bf6c7e2283 | 98 | Thread::wait(2000); |
narshu | 0:f3bf6c7e2283 | 99 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 100 | Thread::wait(5000); |
narshu | 0:f3bf6c7e2283 | 101 | } |
narshu | 0:f3bf6c7e2283 | 102 | } |
narshu | 0:f3bf6c7e2283 | 103 | |
narshu | 0:f3bf6c7e2283 | 104 | |
narshu | 0:f3bf6c7e2283 | 105 | |
narshu | 0:f3bf6c7e2283 | 106 | void vPrintState(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 107 | float state[3]; |
narshu | 0:f3bf6c7e2283 | 108 | float sonardist[3]; |
narshu | 0:f3bf6c7e2283 | 109 | //float x,y; |
narshu | 0:f3bf6c7e2283 | 110 | //float d = beaconpos[2].y - beaconpos[1].y; |
narshu | 0:f3bf6c7e2283 | 111 | //float i = beaconpos[0].y; |
narshu | 0:f3bf6c7e2283 | 112 | //float j = beaconpos[0].x; |
narshu | 0:f3bf6c7e2283 | 113 | |
narshu | 0:f3bf6c7e2283 | 114 | while (1) { |
narshu | 0:f3bf6c7e2283 | 115 | kalman.statelock.lock(); |
narshu | 0:f3bf6c7e2283 | 116 | state[0] = kalman.X(0); |
narshu | 0:f3bf6c7e2283 | 117 | state[1] = kalman.X(1); |
narshu | 0:f3bf6c7e2283 | 118 | state[2] = kalman.X(2); |
narshu | 0:f3bf6c7e2283 | 119 | sonardist[0] = kalman.SonarMeasures[0]*1000.0f; |
narshu | 0:f3bf6c7e2283 | 120 | sonardist[1] = kalman.SonarMeasures[1]*1000.0f; |
narshu | 0:f3bf6c7e2283 | 121 | sonardist[2] = kalman.SonarMeasures[2]*1000.0f; |
narshu | 0:f3bf6c7e2283 | 122 | kalman.statelock.unlock(); |
narshu | 0:f3bf6c7e2283 | 123 | |
narshu | 0:f3bf6c7e2283 | 124 | // trilateration algorithm |
narshu | 0:f3bf6c7e2283 | 125 | //y = ((sonardist[1]*sonardist[1]) - (sonardist[2]*sonardist[2]) + d*d) / (2*d); |
narshu | 0:f3bf6c7e2283 | 126 | //x = ((sonardist[1]*sonardist[1]) - (sonardist[0]*sonardist[0]) + i*i + j*j) / (2*j) - (i/j)*y; |
narshu | 0:f3bf6c7e2283 | 127 | |
narshu | 0:f3bf6c7e2283 | 128 | |
narshu | 0:f3bf6c7e2283 | 129 | //kalman.statelock.lock(); |
narshu | 0:f3bf6c7e2283 | 130 | pc.printf("\r\n"); |
narshu | 0:f3bf6c7e2283 | 131 | //printf("Position X: %0.1f Y: %0.1f \r\n", x,y); |
narshu | 0:f3bf6c7e2283 | 132 | pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]); |
narshu | 0:f3bf6c7e2283 | 133 | //targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 134 | //pc.printf("target : %0.4f %0.4f %0.4f \r\n", targetX/1000, targetY/1000,targetTheta); |
narshu | 0:f3bf6c7e2283 | 135 | //targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 136 | printf("Sonar Dist: %0.4f, %0.4f, %0.4f \r\n", sonardist[0],sonardist[1],sonardist[2]); |
narshu | 0:f3bf6c7e2283 | 137 | |
narshu | 0:f3bf6c7e2283 | 138 | //printf("Distance is %0.1f, %0.1f \r\n", motors.encoderToDistance(motors.getEncoder1()), motors.encoderToDistance(motors.getEncoder2())); |
narshu | 0:f3bf6c7e2283 | 139 | |
narshu | 0:f3bf6c7e2283 | 140 | //pc.printf("IR ang : %f \n",temp * 180/3.14); |
narshu | 0:f3bf6c7e2283 | 141 | //kalman.statelock.unlock(); |
narshu | 0:f3bf6c7e2283 | 142 | Thread::wait(200); |
narshu | 0:f3bf6c7e2283 | 143 | } |
narshu | 0:f3bf6c7e2283 | 144 | } |
narshu | 0:f3bf6c7e2283 | 145 | |
narshu | 0:f3bf6c7e2283 | 146 | |
narshu | 0:f3bf6c7e2283 | 147 | // AI thread ------------------------------------ |
narshu | 0:f3bf6c7e2283 | 148 | void ai_thread (void const *argument) { |
narshu | 0:f3bf6c7e2283 | 149 | // goes to the mid |
narshu | 0:f3bf6c7e2283 | 150 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 151 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 152 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 153 | targetY = 1000; |
narshu | 0:f3bf6c7e2283 | 154 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 155 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 156 | |
narshu | 0:f3bf6c7e2283 | 157 | // left roll |
narshu | 0:f3bf6c7e2283 | 158 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 159 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 160 | targetX = 500; |
narshu | 0:f3bf6c7e2283 | 161 | targetY = 1700; |
narshu | 0:f3bf6c7e2283 | 162 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 163 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 164 | |
narshu | 0:f3bf6c7e2283 | 165 | // mid |
narshu | 0:f3bf6c7e2283 | 166 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 167 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 168 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 169 | targetY = 1000; |
narshu | 0:f3bf6c7e2283 | 170 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 171 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 172 | |
narshu | 0:f3bf6c7e2283 | 173 | // map |
narshu | 0:f3bf6c7e2283 | 174 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 175 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 176 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 177 | targetY = 1700; |
narshu | 0:f3bf6c7e2283 | 178 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 179 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 180 | |
narshu | 0:f3bf6c7e2283 | 181 | // mid |
narshu | 0:f3bf6c7e2283 | 182 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 183 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 184 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 185 | targetY = 1000; |
narshu | 0:f3bf6c7e2283 | 186 | targetTheta = -PI; |
narshu | 0:f3bf6c7e2283 | 187 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 188 | |
narshu | 0:f3bf6c7e2283 | 189 | // home |
narshu | 0:f3bf6c7e2283 | 190 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 191 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 192 | targetX = 500; |
narshu | 0:f3bf6c7e2283 | 193 | targetY = 500; |
narshu | 0:f3bf6c7e2283 | 194 | targetTheta = 0; |
narshu | 0:f3bf6c7e2283 | 195 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 196 | |
narshu | 0:f3bf6c7e2283 | 197 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 198 | flag_terminate = true; |
narshu | 0:f3bf6c7e2283 | 199 | //OLED3 = true; |
narshu | 0:f3bf6c7e2283 | 200 | |
narshu | 0:f3bf6c7e2283 | 201 | while (true) { |
narshu | 0:f3bf6c7e2283 | 202 | Thread::wait(osWaitForever); |
narshu | 0:f3bf6c7e2283 | 203 | } |
narshu | 0:f3bf6c7e2283 | 204 | } |
narshu | 0:f3bf6c7e2283 | 205 | |
narshu | 0:f3bf6c7e2283 | 206 | // motion control thread ------------------------ |
narshu | 0:f3bf6c7e2283 | 207 | void motion_thread(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 208 | motors.resetEncoders(); |
narshu | 0:f3bf6c7e2283 | 209 | motors.setSpeed(20,20); |
narshu | 0:f3bf6c7e2283 | 210 | Thread::wait(2000); |
narshu | 0:f3bf6c7e2283 | 211 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 212 | //thr_AI.signal_set(0x01); |
narshu | 0:f3bf6c7e2283 | 213 | |
narshu | 0:f3bf6c7e2283 | 214 | |
narshu | 0:f3bf6c7e2283 | 215 | |
narshu | 0:f3bf6c7e2283 | 216 | float currX, currY,currTheta; |
narshu | 0:f3bf6c7e2283 | 217 | float speedL,speedR; |
narshu | 0:f3bf6c7e2283 | 218 | float diffDir,diffSpeed; |
narshu | 0:f3bf6c7e2283 | 219 | float lastdiffSpeed = 0; |
narshu | 0:f3bf6c7e2283 | 220 | |
narshu | 0:f3bf6c7e2283 | 221 | while (1) { |
narshu | 0:f3bf6c7e2283 | 222 | if (flag_terminate) { |
narshu | 0:f3bf6c7e2283 | 223 | terminate(); |
narshu | 0:f3bf6c7e2283 | 224 | } |
narshu | 0:f3bf6c7e2283 | 225 | |
narshu | 0:f3bf6c7e2283 | 226 | // get kalman localization estimate ------------------------ |
narshu | 0:f3bf6c7e2283 | 227 | kalman.statelock.lock(); |
narshu | 0:f3bf6c7e2283 | 228 | currX = kalman.X(0)*1000.0f; |
narshu | 0:f3bf6c7e2283 | 229 | currY = kalman.X(1)*1000.0f; |
narshu | 0:f3bf6c7e2283 | 230 | currTheta = kalman.X(2); |
narshu | 0:f3bf6c7e2283 | 231 | kalman.statelock.unlock(); |
narshu | 0:f3bf6c7e2283 | 232 | |
narshu | 0:f3bf6c7e2283 | 233 | |
narshu | 0:f3bf6c7e2283 | 234 | |
narshu | 0:f3bf6c7e2283 | 235 | // get IR turret angle -------------------------------------- |
narshu | 0:f3bf6c7e2283 | 236 | /* float angEst = getAngle(currX, currY); // check this |
narshu | 0:f3bf6c7e2283 | 237 | if (angEst != 1000){ |
narshu | 0:f3bf6c7e2283 | 238 | // here the value should be a valid estimate |
narshu | 0:f3bf6c7e2283 | 239 | // so update kalman state |
narshu | 0:f3bf6c7e2283 | 240 | |
narshu | 0:f3bf6c7e2283 | 241 | } |
narshu | 0:f3bf6c7e2283 | 242 | */ |
narshu | 0:f3bf6c7e2283 | 243 | //!!! add code here to update kalman angle state !!! |
narshu | 0:f3bf6c7e2283 | 244 | |
narshu | 0:f3bf6c7e2283 | 245 | // check if target reached ---------------------------------- |
narshu | 0:f3bf6c7e2283 | 246 | if ( ( abs(currX - targetX) < POSITION_TOR ) |
narshu | 0:f3bf6c7e2283 | 247 | &&( abs(currY - targetY) < POSITION_TOR ) |
narshu | 0:f3bf6c7e2283 | 248 | ) { |
narshu | 0:f3bf6c7e2283 | 249 | |
narshu | 0:f3bf6c7e2283 | 250 | diffDir = rectifyAng(currTheta - targetTheta); |
narshu | 0:f3bf6c7e2283 | 251 | diffSpeed = diffDir / PI; |
narshu | 0:f3bf6c7e2283 | 252 | |
narshu | 0:f3bf6c7e2283 | 253 | //pc.printf("%f \n",diffDir); |
narshu | 0:f3bf6c7e2283 | 254 | if (abs(diffDir) > ANGLE_TOR) { |
narshu | 0:f3bf6c7e2283 | 255 | if (abs(diffSpeed) < 0.5) { |
narshu | 0:f3bf6c7e2283 | 256 | diffSpeed = 0.5*diffSpeed/abs(diffSpeed); |
narshu | 0:f3bf6c7e2283 | 257 | } |
narshu | 0:f3bf6c7e2283 | 258 | motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED)); |
narshu | 0:f3bf6c7e2283 | 259 | |
narshu | 0:f3bf6c7e2283 | 260 | |
narshu | 0:f3bf6c7e2283 | 261 | } else { |
narshu | 0:f3bf6c7e2283 | 262 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 263 | Thread::wait(4000); |
narshu | 0:f3bf6c7e2283 | 264 | //thr_AI.signal_set(0x01); |
narshu | 0:f3bf6c7e2283 | 265 | } |
narshu | 0:f3bf6c7e2283 | 266 | } |
narshu | 0:f3bf6c7e2283 | 267 | |
narshu | 0:f3bf6c7e2283 | 268 | // adjust motion to reach target ---------------------------- |
narshu | 0:f3bf6c7e2283 | 269 | else { |
narshu | 0:f3bf6c7e2283 | 270 | |
narshu | 0:f3bf6c7e2283 | 271 | // calc direction to target |
narshu | 0:f3bf6c7e2283 | 272 | float targetDir = atan2(targetY - currY, targetX - currX); |
narshu | 0:f3bf6c7e2283 | 273 | |
narshu | 0:f3bf6c7e2283 | 274 | |
narshu | 0:f3bf6c7e2283 | 275 | diffDir = rectifyAng(currTheta - targetDir); |
narshu | 0:f3bf6c7e2283 | 276 | diffSpeed = diffDir / PI; |
narshu | 0:f3bf6c7e2283 | 277 | |
narshu | 0:f3bf6c7e2283 | 278 | if (abs(diffDir) > ANGLE_TOR*2) { |
narshu | 0:f3bf6c7e2283 | 279 | if (abs(diffSpeed) < 0.5) { |
narshu | 0:f3bf6c7e2283 | 280 | diffSpeed = 0.5*diffSpeed/abs(diffSpeed); |
narshu | 0:f3bf6c7e2283 | 281 | } |
narshu | 0:f3bf6c7e2283 | 282 | motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED)); |
narshu | 0:f3bf6c7e2283 | 283 | } else { |
narshu | 0:f3bf6c7e2283 | 284 | |
narshu | 0:f3bf6c7e2283 | 285 | |
narshu | 0:f3bf6c7e2283 | 286 | if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) { |
narshu | 0:f3bf6c7e2283 | 287 | if (diffSpeed-lastdiffSpeed >= 0) { |
narshu | 0:f3bf6c7e2283 | 288 | diffSpeed = lastdiffSpeed + MAX_STEP_RATIO; |
narshu | 0:f3bf6c7e2283 | 289 | } else { |
narshu | 0:f3bf6c7e2283 | 290 | diffSpeed = lastdiffSpeed - MAX_STEP_RATIO; |
narshu | 0:f3bf6c7e2283 | 291 | } |
narshu | 0:f3bf6c7e2283 | 292 | } |
narshu | 0:f3bf6c7e2283 | 293 | lastdiffSpeed = diffSpeed; |
narshu | 0:f3bf6c7e2283 | 294 | |
narshu | 0:f3bf6c7e2283 | 295 | // calculte the motor speeds |
narshu | 0:f3bf6c7e2283 | 296 | if (diffSpeed <= 0) { |
narshu | 0:f3bf6c7e2283 | 297 | speedL = (1-2*abs(diffSpeed))*MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 298 | speedR = MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 299 | |
narshu | 0:f3bf6c7e2283 | 300 | } else { |
narshu | 0:f3bf6c7e2283 | 301 | speedR = (1-2*abs(diffSpeed))*MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 302 | speedL = MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 303 | } |
narshu | 0:f3bf6c7e2283 | 304 | |
narshu | 0:f3bf6c7e2283 | 305 | motors.setSpeed( int(speedL), int(speedR)); |
narshu | 0:f3bf6c7e2283 | 306 | } |
narshu | 0:f3bf6c7e2283 | 307 | } |
narshu | 0:f3bf6c7e2283 | 308 | |
narshu | 0:f3bf6c7e2283 | 309 | // wait |
narshu | 0:f3bf6c7e2283 | 310 | Thread::wait(MOTION_UPDATE_PERIOD); |
narshu | 0:f3bf6c7e2283 | 311 | } |
narshu | 0:f3bf6c7e2283 | 312 | } |
narshu | 0:f3bf6c7e2283 | 313 | |
narshu | 0:f3bf6c7e2283 | 314 | |
narshu | 0:f3bf6c7e2283 | 315 | void getIRValue(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 316 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 317 | if (IRturret.readable() >= 24) { |
narshu | 0:f3bf6c7e2283 | 318 | for (int i=0; i < 24; i++) { |
narshu | 0:f3bf6c7e2283 | 319 | IRValues.IR_chars[i] = IRturret.getc(); |
narshu | 0:f3bf6c7e2283 | 320 | } |
narshu | 0:f3bf6c7e2283 | 321 | } |
narshu | 0:f3bf6c7e2283 | 322 | } |
narshu | 0:f3bf6c7e2283 | 323 | |
narshu | 0:f3bf6c7e2283 | 324 | void vIRValueISR (void) { |
narshu | 0:f3bf6c7e2283 | 325 | //thr_getIRValue.signal_set(0x01); |
narshu | 0:f3bf6c7e2283 | 326 | } |
narshu | 0:f3bf6c7e2283 | 327 | |
narshu | 0:f3bf6c7e2283 | 328 | |
narshu | 0:f3bf6c7e2283 | 329 | float getAngle (float x, float y) { |
narshu | 0:f3bf6c7e2283 | 330 | /* function that returns current robot orientation |
narshu | 0:f3bf6c7e2283 | 331 | input: x, y coords in mm |
narshu | 0:f3bf6c7e2283 | 332 | output: orientation in rad |
narshu | 0:f3bf6c7e2283 | 333 | note: the angle readings from IR is read in here |
narshu | 0:f3bf6c7e2283 | 334 | must be called faster than IR turret frequency (few Hz - currently 3Hz) |
narshu | 0:f3bf6c7e2283 | 335 | */ |
narshu | 0:f3bf6c7e2283 | 336 | |
narshu | 0:f3bf6c7e2283 | 337 | // variables |
narshu | 0:f3bf6c7e2283 | 338 | float ang0 = 0, ang1 = 0, ang2 = 0; |
narshu | 0:f3bf6c7e2283 | 339 | int sc0 = 0, sc1 = 0, sc2 = 0; |
narshu | 0:f3bf6c7e2283 | 340 | |
narshu | 0:f3bf6c7e2283 | 341 | |
narshu | 0:f3bf6c7e2283 | 342 | |
narshu | 0:f3bf6c7e2283 | 343 | // read serial port |
narshu | 0:f3bf6c7e2283 | 344 | if (IRturret.readable()) { |
narshu | 0:f3bf6c7e2283 | 345 | IRturret.scanf("%f;%f;%f;%d;%d;%d;",&ang0,&ang1,&ang2,&sc0,&sc1,sc2); |
narshu | 0:f3bf6c7e2283 | 346 | } |
narshu | 0:f3bf6c7e2283 | 347 | |
narshu | 0:f3bf6c7e2283 | 348 | // if none sampled |
narshu | 0:f3bf6c7e2283 | 349 | if (!sc0 && !sc1 && ! sc2) { |
narshu | 0:f3bf6c7e2283 | 350 | return 1000; |
narshu | 0:f3bf6c7e2283 | 351 | } |
narshu | 0:f3bf6c7e2283 | 352 | |
narshu | 0:f3bf6c7e2283 | 353 | // |
narshu | 0:f3bf6c7e2283 | 354 | else { |
narshu | 0:f3bf6c7e2283 | 355 | float est0 = 0, est1 = 0, est2 = 0; |
narshu | 0:f3bf6c7e2283 | 356 | |
narshu | 0:f3bf6c7e2283 | 357 | // calc |
narshu | 0:f3bf6c7e2283 | 358 | if ((sc0 > RELI_BOUND_LOW)||(sc0 < RELI_BOUND_HIGH)) { |
narshu | 0:f3bf6c7e2283 | 359 | est0 = atan2(1000 - y, 3000-x) - ang0; |
narshu | 0:f3bf6c7e2283 | 360 | } |
narshu | 0:f3bf6c7e2283 | 361 | if ((sc1 > RELI_BOUND_LOW)||(sc1 < RELI_BOUND_HIGH)) { |
narshu | 0:f3bf6c7e2283 | 362 | est1 = atan2( -y, -x) - ang1; |
narshu | 0:f3bf6c7e2283 | 363 | } |
narshu | 0:f3bf6c7e2283 | 364 | if ((sc2 > RELI_BOUND_LOW)||(sc2 < RELI_BOUND_HIGH)) { |
narshu | 0:f3bf6c7e2283 | 365 | est2 = atan2(2000 - y, -x) - ang2; |
narshu | 0:f3bf6c7e2283 | 366 | } |
narshu | 0:f3bf6c7e2283 | 367 | |
narshu | 0:f3bf6c7e2283 | 368 | // weighted avg |
narshu | 0:f3bf6c7e2283 | 369 | return rectifyAng( ((est0 * RELI_BOUND_HIGH-sc0) + (est1 * RELI_BOUND_HIGH-sc1) + (est2 * RELI_BOUND_HIGH-sc2)) / (3*RELI_BOUND_HIGH - sc0 - sc1 - sc2) ); |
narshu | 0:f3bf6c7e2283 | 370 | } |
narshu | 0:f3bf6c7e2283 | 371 | |
narshu | 0:f3bf6c7e2283 | 372 | } |
narshu | 0:f3bf6c7e2283 | 373 | |
narshu | 0:f3bf6c7e2283 | 374 |