Robot - Simple controller class for Stinger Robot without…

Users » gtg795y » Code » Robot

Simple controller class for Stinger Robot without using Robotics Connection board.

Robot.cpp@1:b4d202b471ae, 2010-10-13 (annotated)

Committer:: gtg795y
Date:: Wed Oct 13 03:18:54 2010 +0000
Revision:: 1:b4d202b471ae
Parent:: 0:fc95840345e8

Who changed what in which revision?

User	Revision	Line number	New contents of line
gtg795y	0:fc95840345e8	1	#include "Robot.h"
gtg795y	0:fc95840345e8	2	#include "mbed.h"
gtg795y	0:fc95840345e8	3
gtg795y	0:fc95840345e8	4	// This library was designed simplify operation of Stinger robot only with MBED and two H-Bridges.
gtg795y	0:fc95840345e8	5	// Is is still in development.
gtg795y	0:fc95840345e8	6	// Class Robot integrated QEI, PID and Motor classes to work together through use of Ticker function to
gtg795y	0:fc95840345e8	7	// regularly poll QEI objects, adjust PID and Motor objects.
gtg795y	0:fc95840345e8	8	// Right now all pins have been preassigned, later empty constructor will be added with individual pin assignments.
gtg795y	0:fc95840345e8	9	// Pinouts are:
gtg795y	0:fc95840345e8	10	// rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
gtg795y	0:fc95840345e8	11	// leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
gtg795y	0:fc95840345e8	12	// leftQei = new QEI(p29, p30, NC, 624, QEI::X2_ENCODING); //chanA, chanB, index, ppr
gtg795y	0:fc95840345e8	13	// rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING); //chanB, chanA, index, ppr
gtg795y	0:fc95840345e8	14	// leftPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
gtg795y	0:fc95840345e8	15	// rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
gtg795y	0:fc95840345e8	16
gtg795y	0:fc95840345e8	17	Robot::Robot() {
gtg795y	0:fc95840345e8	18	rightMotor = new Motor(p23, p6, p5); // pwm, fwd, rev
gtg795y	0:fc95840345e8	19	leftMotor = new Motor(p22, p8, p7); // pwm, fwd, rev
gtg795y	0:fc95840345e8	20	leftQei = new QEI(p29, p30, NC, 624, QEI::X2_ENCODING); //chanA, chanB, index, ppr
gtg795y	0:fc95840345e8	21	rightQei = new QEI(p27, p28, NC, 624, QEI::X2_ENCODING); //chanB, chanA, index, ppr
gtg795y	0:fc95840345e8	22	leftPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
gtg795y	0:fc95840345e8	23	rightPid = new PID(0.4312, 0.1, 0.0, RATE); //Kc, Ti, Td
gtg795y	0:fc95840345e8	24	leftMotor->period(0.00005); //Set motor PWM periods to 20KHz.
gtg795y	0:fc95840345e8	25	rightMotor->period(0.00005);
gtg795y	0:fc95840345e8	26	leftPid->setInputLimits(0, MAX_SPEED);
gtg795y	0:fc95840345e8	27	leftPid->setOutputLimits(0.3, 1);
gtg795y	0:fc95840345e8	28	leftPid->setMode(AUTO_MODE);
gtg795y	0:fc95840345e8	29	rightPid->setInputLimits(0, MAX_SPEED);
gtg795y	0:fc95840345e8	30	rightPid->setOutputLimits(0.3, 1);
gtg795y	0:fc95840345e8	31	rightPid->setMode(AUTO_MODE);
gtg795y	0:fc95840345e8	32	leftPulsesGoTo=rightPulsesGoTo=0;
gtg795y	0:fc95840345e8	33	ticker.attach(this, &Robot::Call, RATE); // the address of the function to be attached (call) and the interval (RATE)
gtg795y	0:fc95840345e8	34	}
gtg795y	0:fc95840345e8	35
gtg795y	0:fc95840345e8	36	Robot::~Robot() {
gtg795y	0:fc95840345e8	37	delete rightMotor;
gtg795y	0:fc95840345e8	38	delete leftMotor;
gtg795y	0:fc95840345e8	39	delete leftQei;
gtg795y	0:fc95840345e8	40	delete rightQei;
gtg795y	0:fc95840345e8	41	delete leftMotor;
gtg795y	0:fc95840345e8	42	delete rightMotor;
gtg795y	0:fc95840345e8	43	delete leftPid;
gtg795y	0:fc95840345e8	44	delete rightPid;
gtg795y	0:fc95840345e8	45	ticker.detach();
gtg795y	0:fc95840345e8	46	}
gtg795y	0:fc95840345e8	47
gtg795y	0:fc95840345e8	48	void Robot::MoveStraightPulses(float s, int pulses) {
gtg795y	0:fc95840345e8	49	leftQei->reset();
gtg795y	0:fc95840345e8	50	rightQei->reset();
gtg795y	0:fc95840345e8	51	leftPulses=rightPulses=0;
gtg795y	0:fc95840345e8	52	if (s>=0.0)leftDirection=rightDirection=1.0;
gtg795y	0:fc95840345e8	53	else leftDirection=rightDirection=-1.0;
gtg795y	0:fc95840345e8	54	leftSpeed=abs(s*MAX_SPEED);
gtg795y	0:fc95840345e8	55	rightSpeed=abs(s*MAX_SPEED);
gtg795y	0:fc95840345e8	56	leftPid->reset();
gtg795y	0:fc95840345e8	57	rightPid->reset();
gtg795y	0:fc95840345e8	58	leftPid->setSetPoint(leftSpeed);
gtg795y	0:fc95840345e8	59	rightPid->setSetPoint(rightSpeed);
gtg795y	0:fc95840345e8	60	leftPulsesGoTo=rightPulsesGoTo=pulses*2;
gtg795y	0:fc95840345e8	61	if (s==0)Stop();
gtg795y	0:fc95840345e8	62	}
gtg795y	0:fc95840345e8	63
gtg795y	0:fc95840345e8	64	void Robot::MoveStraightInches(float speed, float in){
gtg795y	0:fc95840345e8	65	MoveStraightPulses(speed, (int)(in*85.6144));
gtg795y	0:fc95840345e8	66	}
gtg795y	0:fc95840345e8	67
gtg795y	0:fc95840345e8	68	void Robot::MoveStraightRotations(float speed, float rotations){
gtg795y	0:fc95840345e8	69	MoveStraightPulses(speed, (int)(rotations*624.00));
gtg795y	0:fc95840345e8	70	}
gtg795y	0:fc95840345e8	71
gtg795y	0:fc95840345e8	72	void Robot::Stop() {
gtg795y	0:fc95840345e8	73	this->StopLeft();
gtg795y	0:fc95840345e8	74	this->StopRight();
gtg795y	0:fc95840345e8	75	}
gtg795y	0:fc95840345e8	76
gtg795y	0:fc95840345e8	77	void Robot::StopLeft() {
gtg795y	0:fc95840345e8	78	leftMotor->brake();
gtg795y	0:fc95840345e8	79	leftPulsesGoTo=0;
gtg795y	0:fc95840345e8	80	leftSpeed=0.0;
gtg795y	0:fc95840345e8	81	leftPulses=0;
gtg795y	0:fc95840345e8	82	leftQei->reset();
gtg795y	0:fc95840345e8	83
gtg795y	0:fc95840345e8	84	leftPid->reset();
gtg795y	0:fc95840345e8	85	leftPid->setSetPoint(0);
gtg795y	0:fc95840345e8	86	}
gtg795y	0:fc95840345e8	87
gtg795y	0:fc95840345e8	88	void Robot::StopRight() {
gtg795y	0:fc95840345e8	89	rightMotor->brake();
gtg795y	0:fc95840345e8	90	rightPulsesGoTo=0;
gtg795y	0:fc95840345e8	91	rightSpeed=0.0;
gtg795y	0:fc95840345e8	92	rightPulses=0;
gtg795y	0:fc95840345e8	93	rightQei->reset();
gtg795y	0:fc95840345e8	94	rightPid->reset();
gtg795y	0:fc95840345e8	95	rightPid->setSetPoint(0);
gtg795y	0:fc95840345e8	96
gtg795y	0:fc95840345e8	97
gtg795y	0:fc95840345e8	98	}
gtg795y	0:fc95840345e8	99	void Robot::RotateLeftWheel(float speed, float deg) {
gtg795y	0:fc95840345e8	100	leftPid->reset();
gtg795y	0:fc95840345e8	101	leftSpeed=abs(speed*MAX_SPEED);
gtg795y	0:fc95840345e8	102	if (speed>=0.0)leftDirection=1.0;
gtg795y	0:fc95840345e8	103	else leftDirection=-1.0;
gtg795y	0:fc95840345e8	104	leftPulsesGoTo=deg*624/360;
gtg795y	0:fc95840345e8	105	leftPid->setSetPoint(leftSpeed);
gtg795y	0:fc95840345e8	106	}
gtg795y	0:fc95840345e8	107
gtg795y	0:fc95840345e8	108	void Robot::RotateRightWheel(float speed, float deg) {
gtg795y	0:fc95840345e8	109	rightPid->reset();
gtg795y	0:fc95840345e8	110	rightSpeed=abs(speed*MAX_SPEED);
gtg795y	0:fc95840345e8	111	if (speed>=0.0)rightDirection=1.0;
gtg795y	0:fc95840345e8	112	else leftDirection=-1.0;
gtg795y	0:fc95840345e8	113	rightPulsesGoTo=deg*624/360;
gtg795y	0:fc95840345e8	114	rightPid->setSetPoint(rightSpeed);
gtg795y	0:fc95840345e8	115	}
gtg795y	0:fc95840345e8	116
gtg795y	0:fc95840345e8	117	void Robot::PivetLeft(float deg) {
gtg795y	0:fc95840345e8	118	if (deg<0.0) {
gtg795y	0:fc95840345e8	119	PivetRight(-deg);
gtg795y	0:fc95840345e8	120	return;
gtg795y	0:fc95840345e8	121	}
gtg795y	0:fc95840345e8	122	leftPid->reset();
gtg795y	0:fc95840345e8	123	rightPid->reset();
gtg795y	0:fc95840345e8	124	leftSpeed=abs(0.5*MAX_SPEED);
gtg795y	0:fc95840345e8	125	rightSpeed=abs(0.5*MAX_SPEED);
gtg795y	0:fc95840345e8	126	leftDirection=-1.0;
gtg795y	0:fc95840345e8	127	rightDirection=1.0;
gtg795y	0:fc95840345e8	128	leftPulsesGoTo=abs(deg*DPP);
gtg795y	0:fc95840345e8	129	rightPulsesGoTo=abs(deg*DPP);
gtg795y	0:fc95840345e8	130	leftPid->setSetPoint(leftSpeed);
gtg795y	0:fc95840345e8	131	rightPid->setSetPoint(rightSpeed);
gtg795y	0:fc95840345e8	132	}
gtg795y	0:fc95840345e8	133
gtg795y	0:fc95840345e8	134	void Robot::PivetRight(float deg) {
gtg795y	0:fc95840345e8	135	if (deg<0) {
gtg795y	0:fc95840345e8	136	PivetLeft(-deg);
gtg795y	0:fc95840345e8	137	return;
gtg795y	0:fc95840345e8	138	}
gtg795y	0:fc95840345e8	139	leftPid->reset();
gtg795y	0:fc95840345e8	140	rightPid->reset();
gtg795y	0:fc95840345e8	141	leftSpeed=abs(0.5*MAX_SPEED);
gtg795y	0:fc95840345e8	142	rightSpeed=abs(0.5*MAX_SPEED);
gtg795y	0:fc95840345e8	143	leftDirection=1.0;
gtg795y	0:fc95840345e8	144	rightDirection=-1.0;
gtg795y	0:fc95840345e8	145	leftPulsesGoTo=abs(deg*DPP);
gtg795y	0:fc95840345e8	146	rightPulsesGoTo=abs(deg*DPP);
gtg795y	0:fc95840345e8	147	leftPid->setSetPoint(leftSpeed);
gtg795y	0:fc95840345e8	148	rightPid->setSetPoint(rightSpeed);
gtg795y	0:fc95840345e8	149	}
gtg795y	0:fc95840345e8	150
gtg795y	0:fc95840345e8	151	int Robot::IsBusy() {
gtg795y	0:fc95840345e8	152	if ((rightSpeed==0.0)&&(leftSpeed==0.0))return 0;
gtg795y	0:fc95840345e8	153	return 1;
gtg795y	0:fc95840345e8	154	}
gtg795y	0:fc95840345e8	155
gtg795y	0:fc95840345e8	156	void Robot::Call() {
gtg795y	0:fc95840345e8	157	float t;
gtg795y	0:fc95840345e8	158	if ((rightPulsesGoTo!=0)&&(rightSpeed>0)) {
gtg795y	0:fc95840345e8	159	if (rightPulses < rightPulsesGoTo) {
gtg795y	0:fc95840345e8	160	rightPulses = (int)rightDirection * rightQei->getPulses();
gtg795y	0:fc95840345e8	161	rightVelocity = (rightPulses - rightPrevPulses) / RATE;
gtg795y	0:fc95840345e8	162	rightPrevPulses = rightPulses;
gtg795y	0:fc95840345e8	163	t=rightPid->compute();
gtg795y	0:fc95840345e8	164	if(((rightPulsesGoTo-rightPulses)/rightVelocity)<1.0){
gtg795y	0:fc95840345e8	165	rightPid->reset();
gtg795y	0:fc95840345e8	166	rightPid->setSetPoint(t*2000);
gtg795y	0:fc95840345e8	167	}
gtg795y	0:fc95840345e8	168	rightPid->setProcessValue(fabs(rightVelocity));
gtg795y	0:fc95840345e8	169	rightMotor->speed(rightDirection * t);
gtg795y	0:fc95840345e8	170	} else {
gtg795y	0:fc95840345e8	171	rightPulsesGoTo=0;
gtg795y	0:fc95840345e8	172	this->StopRight();
gtg795y	0:fc95840345e8	173	}
gtg795y	0:fc95840345e8	174	} else {
gtg795y	0:fc95840345e8	175	rightPulsesGoTo=0;
gtg795y	0:fc95840345e8	176	this->StopRight();
gtg795y	0:fc95840345e8	177
gtg795y	0:fc95840345e8	178	}
gtg795y	0:fc95840345e8	179	if ((leftPulsesGoTo!=0)&&(leftSpeed>0)) {
gtg795y	0:fc95840345e8	180	if (leftPulses < leftPulsesGoTo) {
gtg795y	0:fc95840345e8	181	leftPulses = (int)leftDirection * leftQei->getPulses();
gtg795y	0:fc95840345e8	182	leftVelocity = (leftPulses - leftPrevPulses) / RATE;
gtg795y	0:fc95840345e8	183	leftPrevPulses = leftPulses;
gtg795y	0:fc95840345e8	184	t=leftPid->compute();
gtg795y	0:fc95840345e8	185	if(((leftPulsesGoTo-leftPulses)/leftVelocity)<1.0){
gtg795y	0:fc95840345e8	186	leftPid->reset();
gtg795y	0:fc95840345e8	187	leftPid->setSetPoint(t*2000);
gtg795y	0:fc95840345e8	188	}
gtg795y	0:fc95840345e8	189	leftPid->setProcessValue(fabs(leftVelocity));
gtg795y	0:fc95840345e8	190	leftMotor->speed(leftDirection * t);
gtg795y	0:fc95840345e8	191	} else {
gtg795y	0:fc95840345e8	192	leftPulsesGoTo=0;
gtg795y	0:fc95840345e8	193	this->StopLeft();
gtg795y	0:fc95840345e8	194	}
gtg795y	0:fc95840345e8	195	} else {
gtg795y	0:fc95840345e8	196	leftPulsesGoTo=0;
gtg795y	0:fc95840345e8	197	this->StopLeft();
gtg795y	0:fc95840345e8	198	}
gtg795y	0:fc95840345e8	199	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	12 Oct 2010
Imports:	85
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	0
Followers:	11

Robot.cpp@1:b4d202b471ae, 2010-10-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning