EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

position-sys-cam: keep tilt when going to position

pull/1/head
EmaMaker 2022-06-26 19:33:08 +02:00
parent 26c3ec53e3
commit c8997bfc73
1 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/system/positions/positionsys_camera.cpp
View File

@ -5,6 +5,13 @@
#include "math.h"
int diff(int a, int b){
int diffB = abs(min(a, b) - max(a, b));
int diffB1 = 360-diffB;
int diff = min(diffB, diffB1);
return diff;
}
PositionSysCamera::PositionSysCamera() {
MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
@ -172,11 +179,15 @@ void PositionSysCamera::CameraPID(){
CURRENT_DATA_WRITE.addvx = vx;
CURRENT_DATA_WRITE.addvy = vy;
#else
drive->prepareDrive(dir, speed, 0);
int tmp = (CURRENT_DATA_READ.tilt+360)%360;
dir = dir-tmp;
if(dir < 0) dir+=360;
drive->prepareDrive(dir , speed, CURRENT_DATA_WRITE.tilt);
#endif
}
}
void PositionSysCamera::test(){
}