Simple controller class for Stinger Robot without using Robotics Connection board.
Robot.cpp
- Committer:
- gtg795y
- Date:
- 2010-10-12
- Revision:
- 0:fc95840345e8
File content as of revision 0:fc95840345e8:
#include "Robot.h" #include "mbed.h" // This library was designed simplify operation of Stinger robot only with MBED and two H-Bridges. // Is is still in development. // Class Robot integrated QEI, PID and Motor classes to work together through use of Ticker function to // regularly poll QEI objects, adjust PID and Motor objects. // Right now all pins have been preassigned, later empty constructor will be added with individual pin assignments. // Pinouts are: // rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev // leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev // leftQei = new QEI(p29, p30, NC, 624, QEI::X2_ENCODING); //chanA, chanB, index, ppr // rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING); //chanB, chanA, index, ppr // leftPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td // rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td Robot::Robot() { rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev leftQei = new QEI(p29, p30, NC, 624, QEI::X2_ENCODING); //chanA, chanB, index, ppr rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING); //chanB, chanA, index, ppr leftPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td leftMotor->period(0.00005); //Set motor PWM periods to 20KHz. rightMotor->period(0.00005); leftPid->setInputLimits(0, MAX_SPEED); leftPid->setOutputLimits(0.3, 1); leftPid->setMode(AUTO_MODE); rightPid->setInputLimits(0, MAX_SPEED); rightPid->setOutputLimits(0.3, 1); rightPid->setMode(AUTO_MODE); leftPulsesGoTo=rightPulsesGoTo=0; ticker.attach(this, &Robot::Call, RATE); // the address of the function to be attached (call) and the interval (RATE) } Robot::~Robot() { delete rightMotor; delete leftMotor; delete leftQei; delete rightQei; delete leftMotor; delete rightMotor; delete leftPid; delete rightPid; ticker.detach(); } void Robot::MoveStraightPulses(float s, int pulses) { leftQei->reset(); rightQei->reset(); leftPulses=rightPulses=0; if (s>=0.0)leftDirection=rightDirection=1.0; else leftDirection=rightDirection=-1.0; leftSpeed=abs(s*MAX_SPEED); rightSpeed=abs(s*MAX_SPEED); leftPid->reset(); rightPid->reset(); leftPid->setSetPoint(leftSpeed); rightPid->setSetPoint(rightSpeed); leftPulsesGoTo=rightPulsesGoTo=pulses*2; if (s==0)Stop(); } void Robot::MoveStraightInches(float speed, float in){ MoveStraightPulses(speed, (int)(in*85.6144)); } void Robot::MoveStraightRotations(float speed, float rotations){ MoveStraightPulses(speed, (int)(rotations*624.00)); } void Robot::Stop() { this->StopLeft(); this->StopRight(); } void Robot::StopLeft() { leftMotor->brake(); leftPulsesGoTo=0; leftSpeed=0.0; leftPulses=0; leftQei->reset(); leftPid->reset(); leftPid->setSetPoint(0); } void Robot::StopRight() { rightMotor->brake(); rightPulsesGoTo=0; rightSpeed=0.0; rightPulses=0; rightQei->reset(); rightPid->reset(); rightPid->setSetPoint(0); } void Robot::RotateLeftWheel(float speed, float deg) { leftPid->reset(); leftSpeed=abs(speed*MAX_SPEED); if (speed>=0.0)leftDirection=1.0; else leftDirection=-1.0; leftPulsesGoTo=deg*624/360; leftPid->setSetPoint(leftSpeed); } void Robot::RotateRightWheel(float speed, float deg) { rightPid->reset(); rightSpeed=abs(speed*MAX_SPEED); if (speed>=0.0)rightDirection=1.0; else leftDirection=-1.0; rightPulsesGoTo=deg*624/360; rightPid->setSetPoint(rightSpeed); } void Robot::PivetLeft(float deg) { if (deg<0.0) { PivetRight(-deg); return; } leftPid->reset(); rightPid->reset(); leftSpeed=abs(0.5*MAX_SPEED); rightSpeed=abs(0.5*MAX_SPEED); leftDirection=-1.0; rightDirection=1.0; leftPulsesGoTo=abs(deg*DPP); rightPulsesGoTo=abs(deg*DPP); leftPid->setSetPoint(leftSpeed); rightPid->setSetPoint(rightSpeed); } void Robot::PivetRight(float deg) { if (deg<0) { PivetLeft(-deg); return; } leftPid->reset(); rightPid->reset(); leftSpeed=abs(0.5*MAX_SPEED); rightSpeed=abs(0.5*MAX_SPEED); leftDirection=1.0; rightDirection=-1.0; leftPulsesGoTo=abs(deg*DPP); rightPulsesGoTo=abs(deg*DPP); leftPid->setSetPoint(leftSpeed); rightPid->setSetPoint(rightSpeed); } int Robot::IsBusy() { if ((rightSpeed==0.0)&&(leftSpeed==0.0))return 0; return 1; } void Robot::Call() { float t; if ((rightPulsesGoTo!=0)&&(rightSpeed>0)) { if (rightPulses < rightPulsesGoTo) { rightPulses = (int)rightDirection * rightQei->getPulses(); rightVelocity = (rightPulses - rightPrevPulses) / RATE; rightPrevPulses = rightPulses; t=rightPid->compute(); if(((rightPulsesGoTo-rightPulses)/rightVelocity)<1.0){ rightPid->reset(); rightPid->setSetPoint(t*2000); } rightPid->setProcessValue(fabs(rightVelocity)); rightMotor->speed(rightDirection * t); } else { rightPulsesGoTo=0; this->StopRight(); } } else { rightPulsesGoTo=0; this->StopRight(); } if ((leftPulsesGoTo!=0)&&(leftSpeed>0)) { if (leftPulses < leftPulsesGoTo) { leftPulses = (int)leftDirection * leftQei->getPulses(); leftVelocity = (leftPulses - leftPrevPulses) / RATE; leftPrevPulses = leftPulses; t=leftPid->compute(); if(((leftPulsesGoTo-leftPulses)/leftVelocity)<1.0){ leftPid->reset(); leftPid->setSetPoint(t*2000); } leftPid->setProcessValue(fabs(leftVelocity)); leftMotor->speed(leftDirection * t); } else { leftPulsesGoTo=0; this->StopLeft(); } } else { leftPulsesGoTo=0; this->StopLeft(); } }