Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

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?

UserRevisionLine numberNew 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