/*
-Pre-Reqs: SetupPWM(uint32_t prescale) with prescale = 10
setupI2C1()
setupUART()
setupADC()
-Globals: sweepspeed
samplespersweep
sweepstartpos
sweepstoppos
-Desc: Moves servo between servo start and end position at a set speed,
scans a set number of times.
If '0' is pressed on the keypad, quits.
*/
void scanMode(void){
initServo(1);
setServo(sweepstartpos);
SystickTimer(100);
// gap between scans = scan range / sample speed
uint32_t scangap = (uint32_t)((float)(sweepstoppos - sweepstartpos)/(float)samplespersweep);
char button;
int turnAmt;
int dir = 1;
int i = samplespersweep - 1;
while(1){
// If servo at edge, change direction
if(servoAtEdge()){
dir = dir*-1;
startSweep();
i = samplespersweep - 1;
averageDistance = _averageSweep();
}
// Turn servo in the "dir" direction by "sweepspeed"
turnAmt = dir*sweepspeed;
turnServo(turnAmt);
if(servo_position % scangap < sweepspeed){
currentIr = irMedian();
sendScanData(currentIr);
sweep[i] = currentIr;
currentRawIr = irMedianRaw();
}
// If 0 was pressed on the keypad, break out of the loop.
button = KeypadTest();
if(button == '0'){
break;
};
SystickTimer(10);
}
}
void SweepHelper(Mesh& mesh) {
float r_i = 0.2;
float r_o = 0.7;
glm::vec3 startSweep(0.0f, 0.0f, 1.0f);
glm::vec3 endSweep(0.0f, 0.0f, 3.5f);
// the sweep curve.
auto sweepCurve = [=](float t) {
return
(1.0f-t)*startSweep + t * endSweep
// glm::vec3(0.0f, -0.9f * t*t, 0.0f );
// glm::vec3(0.0f,0.5f,0.0f) * (float)sin(0.8f*2.0f*M_PI * t);
; };
const float STEP_LENGTH = 0.01;
for(float t = 0.0f; t <= 1.0; t+=STEP_LENGTH) {
float t2 = t+STEP_LENGTH;
float t1 = t;
glm::vec3 c = sweepCurve(t1);
glm::vec3 v = sweepCurve(t2) - sweepCurve(t1);
float deltaLength = glm::length(v);
v = glm::normalize(v);
glm::vec3 u;
glm::vec3 w;
FindBasis(v, u, w);
auto e = [&](glm::vec3 x) { return glm::dot(u, x-c ); };
auto f = [&](glm::vec3 x) { return glm::dot(w, x-c ); };
auto b = [=](float rx) {
float a = (rx - r_i) / (r_o - r_i);
return 3*a*a*a*a - 4*a*a*a + 1;
};
auto p = [&](const glm::vec3& x) {
float rx = glm::length(x - c);
if(rx < r_i) {
return e(x);
}else if(rx >= r_i && rx <= r_o) {
return e(x) * b(rx);
} else {
return 0.0f;
}
};
auto q = [&](const glm::vec3& x) {
float rx = glm::length(x - c);
if(rx < r_i) {
return f(x);
}else if(rx >= r_i && rx <= r_o) {
return f(x) * b(rx);
} else {
return 0.0f;
}
};
for(glm::vec3& x : mesh.vertices) {
glm::vec3 grad_p = Gradient(p, x );
glm::vec3 grad_q = Gradient(q, x );
/*
float rx = glm::length(x - c);
float s = 1.0f;
if(rx < r_i) {
s *= deltaLength;
} else if(rx >= r_i && rx <= r_o) {
s *= b(rx);
} else {
s *= 0.0f;
}
// deformation field.
//glm::vec3 D = (glm::cross(div_e, div_f));
glm::vec3 D = s * v;
*/
glm::vec3 D = deltaLength * glm::cross(grad_q, grad_p );
x += D;
}
}
}
