bool isPalindrome(ListNode* head) {
// check for special cases.
if (!head || !head->next)
return true;
ListNode *p, *dp, *bkp;
// find the middle point
p = dp = head;
while( p && dp) {
p = p->next; // increment single pointer once.
dp = dp->next; //increment double pointer twice.
if (dp) // if possible.
dp = dp->next;
}
// reverse the list here.
bkp = dp = reverseR(p);
p = head;
#if __RESTORE_LIST
bool match = true;
// traverse again and see if they match completely.
while (p && dp) {
if (match && p->val != dp->val)
match = false;
dp = dp->next;
if (p->next) // dp should exhaust first.
p = p->next;
}
// restore the netlist.
if (p->next)
p = p->next;
//assert(!p->next); // enable only in debugging & validation mode.
p->next = reverseR(bkp);
return match;
#else
while (p && dp) {
if (p->val != dp->val)
return false;
p = p->next;
dp = dp->next;
}
return true;
#endif
}
ListNode *reverseR(ListNode *head) {
if (!head || !head->next)
return head;
ListNode *tmp = reverseR(head->next);
head->next->next = head;
head->next = NULL;
return tmp;
}
void Pheeno::PIDMotorControl(float desLVel, float desRVel){
/*Keeps the rotational speeds of the individual motors at setpoints desLVel and desRVel (rad/s).*/
float timeStep = 50;
if (millis() - PIDMotorsTimeStart >= timeStep){
float PIDTimeStep = (millis() - PIDMotorsTimeStart)/1000;//Time step for controller to work on (s).
readEncoders();
int countL = encoderCountL;
int countR = encoderCountR;
// Error on individual motors for vel control
float errorL = desLVel - 2 * pi * (countL - oldMotorPIDEncoderCountL) / (encoderCountsPerRotation * motorGearRatio * PIDTimeStep);
float errorR = desRVel - 2 * pi * (countR - oldMotorPIDEncoderCountR) / (encoderCountsPerRotation * motorGearRatio * PIDTimeStep);
float integralL = integralL + errorL * PIDTimeStep;
float integralR = integralR + errorR * PIDTimeStep;
float diffL = (oldErrorL - errorL) / PIDTimeStep;
float diffR = (oldErrorR - errorR) / PIDTimeStep;
oldErrorL = errorL;
oldErrorR = errorR;
oldMotorPIDEncoderCountL = countL;
oldMotorPIDEncoderCountR = countR;
motorL += int(motorDigitalK*(kpMotor*errorL + kiMotor*integralL + kdMotor*diffL));
motorR += int(motorDigitalK*(kpMotor*errorR + kiMotor*integralR + kdMotor*diffR));
if (motorL>255){
motorL=255;
}
if (motorR>255){
motorR=255;
}
if (motorL<-255){
motorL=-255;
}
if (motorR<-255){
motorR=-255;
}
if (motorL >= 0){
forwardL(motorL);
}
if (motorR >= 0){
forwardR(motorR);
}
if (motorL < 0){
reverseL(abs(motorL));
}
if (motorR < 0){
reverseR(abs(motorR));
}
PIDMotorsTimeStart = millis();
}
}
void Pheeno::turnRight(int motorSpeed){
reverseR(motorSpeed);
forwardL(motorSpeed);
}
void Pheeno::reverse(int motorSpeed){
// Both motors drive backwards.
reverseL(motorSpeed);
reverseR(motorSpeed);
}
