forked from MDL29/JacoBot
[NOT WORKING] I think I found thomething...
This commit is contained in:
parent
8294b9c0d1
commit
ea3a44c024
|
@ -37,52 +37,26 @@ void Car::turn(float angle, double setPoint) { //angle : degre
|
||||||
const float m1_mmp = pi*(*m_m1).m_radius*2/(*m_m1).m_ratio; // mm/pulse for m1
|
const float m1_mmp = pi*(*m_m1).m_radius*2/(*m_m1).m_ratio; // mm/pulse for m1
|
||||||
const float m2_mmp = pi*(*m_m2).m_radius*2/(*m_m2).m_ratio; // mm/pulse for m2
|
const float m2_mmp = pi*(*m_m2).m_radius*2/(*m_m2).m_ratio; // mm/pulse for m2
|
||||||
double m1_duration = 0;
|
double m1_duration = 0;
|
||||||
double m2_duration = 0;
|
|
||||||
m_m1->setPoint = setPoint;
|
m_m1->setPoint = setPoint;
|
||||||
m_m2->setPoint = setPoint/m2_mmp*m1_mmp;
|
m_m2->setPoint = setPoint/m2_mmp*m1_mmp;
|
||||||
boolean m1_tmp = false;
|
|
||||||
boolean m2_tmp = false;
|
|
||||||
if(angle>0) {
|
if(angle>0) {
|
||||||
while(!m1_tmp && !m2_tmp) {
|
while(m1_duration<distance/m1_mmp) {
|
||||||
if(m1_duration<distance/m1_mmp) {
|
m1_duration+=m_m1->duration;
|
||||||
// Serial.print("M1 cycle ");
|
m_m1->compute(0);
|
||||||
// Serial.print(m1_duration);
|
m_m1->advance();
|
||||||
// Serial.print(" ");
|
m_m2->compute(0);
|
||||||
// Serial.println(m_m1->duration);
|
m_m2->back();
|
||||||
m1_duration+=m_m1->duration;
|
|
||||||
m_m1->compute(0);
|
|
||||||
m_m1->advance();
|
|
||||||
m_m1->duration = 0;
|
|
||||||
} else {m1_tmp = true; m_m1->stop();}
|
|
||||||
if(m2_duration<distance/m2_mmp) {
|
|
||||||
// Serial.print("M2 cycle ");
|
|
||||||
// Serial.print(m2_duration);
|
|
||||||
// Serial.print(" ");
|
|
||||||
// Serial.println(m_m2->duration);
|
|
||||||
m2_duration-=m_m2->duration;
|
|
||||||
m_m2->compute(0);
|
|
||||||
m_m2->back();
|
|
||||||
m_m2->duration = 0;
|
|
||||||
} else {m2_tmp = true; m_m2->stop();}
|
|
||||||
}
|
}
|
||||||
} else if(angle<0) {
|
} else if(angle<0) {
|
||||||
while(!m1_tmp && !m2_tmp) {
|
while(m1_duration<distance/m1_mmp) {
|
||||||
if(m1_duration<distance/m1_mmp) {
|
m1_duration+=m_m1->duration;
|
||||||
m1_duration-=m_m1->duration;
|
m_m1->compute(0);
|
||||||
m_m1->compute(0);
|
m_m1->back();
|
||||||
m_m1->back();
|
m_m2->compute(0);
|
||||||
m_m1->duration = 0;
|
m_m2->advance();
|
||||||
} else {m1_tmp = true; m_m1->stop();}
|
|
||||||
if(m2_duration<distance/m2_mmp) {
|
|
||||||
m2_duration+=m_m2->duration;
|
|
||||||
m_m2->compute(0);
|
|
||||||
m_m2->advance();
|
|
||||||
m_m2->duration = 0;
|
|
||||||
} else {m2_tmp = true; m_m2->stop();}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
Serial.println(m1_tmp);
|
|
||||||
Serial.println("End");
|
|
||||||
m_m1->stop();
|
m_m1->stop();
|
||||||
m_m2->stop();
|
m_m2->stop();
|
||||||
|
return;
|
||||||
}
|
}
|
|
@ -56,6 +56,7 @@ void MotorEncoder::back() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotorEncoder::stop() {
|
void MotorEncoder::stop() {
|
||||||
|
digitalWrite(m_M_left, LOW);
|
||||||
analogWrite(m_E_left, 0);
|
analogWrite(m_E_left, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,4 +30,5 @@ void setup() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
|
Serial.println(m1.duration);
|
||||||
}
|
}
|
Loading…
Reference in New Issue
Block a user