Robot - | Mbed

Users » sowmy87 » Code » Robot

Robot.cpp@2:7146fc352133, 2010-12-16 (annotated)

Committer:: sowmy87
Date:: Thu Dec 16 09:40:55 2010 +0000
Revision:: 2:7146fc352133
Parent:: 1:8fa2ee8c005e
Child:: 3:36c4d1dbed7e

Who changed what in which revision?

User	Revision	Line number	New contents of line
sowmy87	0:637fe5c74b2d	1	#ifndef __ROBOT__C
sowmy87	0:637fe5c74b2d	2	#define __ROBOT__C
sowmy87	0:637fe5c74b2d	3
sowmy87	0:637fe5c74b2d	4	#include "Robot.h"
sowmy87	0:637fe5c74b2d	5
sowmy87	0:637fe5c74b2d	6	SRF05 sensor(p24,p25);
sowmy87	0:637fe5c74b2d	7
sowmy87	0:637fe5c74b2d	8
sowmy87	0:637fe5c74b2d	9	Serial pcr(USBTX, USBRX);
sowmy87	0:637fe5c74b2d	10
sowmy87	0:637fe5c74b2d	11	Robot::Robot() {
sowmy87	0:637fe5c74b2d	12	led1 = new DigitalOut(LED1);
sowmy87	0:637fe5c74b2d	13	led2 = new DigitalOut(LED2);
sowmy87	0:637fe5c74b2d	14	led3 = new DigitalOut(LED3);
sowmy87	0:637fe5c74b2d	15
sowmy87	0:637fe5c74b2d	16	compass=new CMPS03(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
sowmy87	0:637fe5c74b2d	17	wait(1);
sowmy87	0:637fe5c74b2d	18	desiredHeading=compass->readBearing();
sowmy87	0:637fe5c74b2d	19
sowmy87	0:637fe5c74b2d	20	pid = new PID(1.0, 0.00, 0.01, PID_RATE);
sowmy87	0:637fe5c74b2d	21	pid->setInputLimits(-450,450);
sowmy87	0:637fe5c74b2d	22	pid->setOutputLimits(-MAX_ROBOT_SPEED,MAX_ROBOT_SPEED);
sowmy87	0:637fe5c74b2d	23	pid->setBias(0);
sowmy87	0:637fe5c74b2d	24	pid->setMode(AUTO_MODE);
sowmy87	0:637fe5c74b2d	25	pid->setSetPoint(0);
sowmy87	0:637fe5c74b2d	26
sowmy87	0:637fe5c74b2d	27	range= new float[55];
sowmy87	0:637fe5c74b2d	28	returnRange = new float[11];
sowmy87	0:637fe5c74b2d	29	// sensor1=new SRF05 (p22, p23);
sowmy87	0:637fe5c74b2d	30	// sensor=new SRF05 (p24, p25);
sowmy87	0:637fe5c74b2d	31
sowmy87	0:637fe5c74b2d	32	isFullScan=0;
sowmy87	0:637fe5c74b2d	33	readingNumber=0;
sowmy87	0:637fe5c74b2d	34
sowmy87	0:637fe5c74b2d	35	servo=new Servo (p21);
sowmy87	0:637fe5c74b2d	36	servo->calibrate(.0009, 180);
sowmy87	0:637fe5c74b2d	37	servoPosition=5;
sowmy87	0:637fe5c74b2d	38	servoDirection=1;
sowmy87	0:637fe5c74b2d	39	*servo=(float)servoPosition/10;
sowmy87	0:637fe5c74b2d	40	wait(1);
sowmy87	0:637fe5c74b2d	41
sowmy87	0:637fe5c74b2d	42
sowmy87	0:637fe5c74b2d	43
sowmy87	0:637fe5c74b2d	44	ticker1.attach(this, &Robot::TickTick, PID_RATE*2);
sowmy87	1:8fa2ee8c005e	45	// ticker2.attach(this, &Robot::TackTack, SERVO_RATE);
sowmy87	0:637fe5c74b2d	46	ticker3.attach(this, &Robot::FlashLEDs, 1);
sowmy87	0:637fe5c74b2d	47	}
sowmy87	0:637fe5c74b2d	48	Robot::~Robot() {
sowmy87	0:637fe5c74b2d	49	delete compass;
sowmy87	0:637fe5c74b2d	50	delete pid;
sowmy87	0:637fe5c74b2d	51	// delete sensor;
sowmy87	0:637fe5c74b2d	52	delete range;
sowmy87	0:637fe5c74b2d	53	delete returnRange;
sowmy87	0:637fe5c74b2d	54	delete servo;
sowmy87	0:637fe5c74b2d	55	}
sowmy87	0:637fe5c74b2d	56
sowmy87	0:637fe5c74b2d	57	int Robot::GetDesiredHeading() {
sowmy87	0:637fe5c74b2d	58	return desiredHeading/10;
sowmy87	0:637fe5c74b2d	59	}
sowmy87	0:637fe5c74b2d	60	int Robot::GetActualHeading() {
sowmy87	0:637fe5c74b2d	61	return actualHeading/10;
sowmy87	0:637fe5c74b2d	62	}
sowmy87	0:637fe5c74b2d	63	int Robot::SetDediredHeading(int deg) {
sowmy87	0:637fe5c74b2d	64	deg%=360;
sowmy87	0:637fe5c74b2d	65	if (deg<0)deg=deg+360;
sowmy87	0:637fe5c74b2d	66	desiredHeading=deg*10;
sowmy87	0:637fe5c74b2d	67	return deg;
sowmy87	0:637fe5c74b2d	68	}
sowmy87	0:637fe5c74b2d	69
sowmy87	0:637fe5c74b2d	70	void Robot::SetSpeed(int spd) {
sowmy87	0:637fe5c74b2d	71	int tmp=0;
sowmy87	0:637fe5c74b2d	72	desiredSpeed=spd%MAX_ROBOT_SPEED;
sowmy87	0:637fe5c74b2d	73	if (desiredSpeed>0)tmp=desiredSpeed+50;
sowmy87	0:637fe5c74b2d	74	if (desiredSpeed<0)tmp=desiredSpeed-50;
sowmy87	0:637fe5c74b2d	75	base.SetPWM(tmp);
sowmy87	0:637fe5c74b2d	76	}
sowmy87	0:637fe5c74b2d	77
sowmy87	0:637fe5c74b2d	78	void Robot::SetSpeed(int lSpd, int rSpd) {
sowmy87	0:637fe5c74b2d	79	lSpd%=MAX_ROBOT_SPEED;
sowmy87	0:637fe5c74b2d	80	rSpd%=MAX_ROBOT_SPEED;
sowmy87	0:637fe5c74b2d	81	if (lSpd>0)lSpd+=50;
sowmy87	0:637fe5c74b2d	82	if (lSpd<0)lSpd-=50;
sowmy87	0:637fe5c74b2d	83	if (rSpd>0)rSpd+=50;
sowmy87	0:637fe5c74b2d	84	if (rSpd<0)rSpd-=50;
sowmy87	0:637fe5c74b2d	85	base.SetPWM(lSpd,rSpd);
sowmy87	0:637fe5c74b2d	86	}
sowmy87	0:637fe5c74b2d	87	void Robot::SetLeftSpeed(int spd) {
sowmy87	0:637fe5c74b2d	88	spd%=MAX_ROBOT_SPEED;
sowmy87	0:637fe5c74b2d	89	if (spd>0)spd+=50;
sowmy87	0:637fe5c74b2d	90	if (spd<0)spd-=50;
sowmy87	0:637fe5c74b2d	91	base.SetLeftPWM(spd);
sowmy87	0:637fe5c74b2d	92	}
sowmy87	0:637fe5c74b2d	93
sowmy87	0:637fe5c74b2d	94	void Robot::SetRightSpeed(int spd) {
sowmy87	0:637fe5c74b2d	95	spd%=MAX_ROBOT_SPEED;
sowmy87	0:637fe5c74b2d	96	if (spd>0)spd+=50;
sowmy87	0:637fe5c74b2d	97	if (spd<0)spd-=50;
sowmy87	0:637fe5c74b2d	98	// pcr.printf("R:%i\t\t",spd);
sowmy87	0:637fe5c74b2d	99	base.SetRightPWM(spd);
sowmy87	0:637fe5c74b2d	100	}
sowmy87	0:637fe5c74b2d	101
sowmy87	1:8fa2ee8c005e	102	void Robot::Stop() {
sowmy87	1:8fa2ee8c005e	103	SetSpeed(0);
sowmy87	1:8fa2ee8c005e	104	}
sowmy87	1:8fa2ee8c005e	105
sowmy87	1:8fa2ee8c005e	106	void Robot::ClearCount() {
sowmy87	1:8fa2ee8c005e	107	base.ClearCount();
sowmy87	1:8fa2ee8c005e	108	}
sowmy87	1:8fa2ee8c005e	109
sowmy87	1:8fa2ee8c005e	110	float Robot::GetDistance() {
sowmy87	1:8fa2ee8c005e	111	return base.GetDistance();
sowmy87	1:8fa2ee8c005e	112	}
sowmy87	1:8fa2ee8c005e	113
sowmy87	1:8fa2ee8c005e	114	int Robot::GetCount() {
sowmy87	1:8fa2ee8c005e	115	return base.GetCount();
sowmy87	1:8fa2ee8c005e	116	}
sowmy87	1:8fa2ee8c005e	117
sowmy87	2:7146fc352133	118	void Robot::PivetRight(int deg){
sowmy87	2:7146fc352133	119
sowmy87	2:7146fc352133	120	}
sowmy87	2:7146fc352133	121
sowmy87	2:7146fc352133	122	void Robot::PivetLeft(int deg){
sowmy87	2:7146fc352133	123
sowmy87	2:7146fc352133	124	}
sowmy87	2:7146fc352133	125
sowmy87	0:637fe5c74b2d	126	float Robot::GetRange(int position) {
sowmy87	0:637fe5c74b2d	127	switch (position) {
sowmy87	0:637fe5c74b2d	128	case 0:
sowmy87	0:637fe5c74b2d	129	return range0/5;
sowmy87	0:637fe5c74b2d	130	case 1:
sowmy87	0:637fe5c74b2d	131	return range1/5;
sowmy87	0:637fe5c74b2d	132	case 2:
sowmy87	0:637fe5c74b2d	133	return range2/5;
sowmy87	0:637fe5c74b2d	134	case 3:
sowmy87	0:637fe5c74b2d	135	return range3/5;
sowmy87	0:637fe5c74b2d	136	case 4:
sowmy87	0:637fe5c74b2d	137	return range4/5;
sowmy87	0:637fe5c74b2d	138	case 5:
sowmy87	0:637fe5c74b2d	139	return range5/5;
sowmy87	0:637fe5c74b2d	140	case 6:
sowmy87	0:637fe5c74b2d	141	return range6/5;
sowmy87	0:637fe5c74b2d	142	case 7:
sowmy87	0:637fe5c74b2d	143	return range7/5;
sowmy87	0:637fe5c74b2d	144	case 8:
sowmy87	0:637fe5c74b2d	145	return range8/5;
sowmy87	0:637fe5c74b2d	146	case 9:
sowmy87	0:637fe5c74b2d	147	return range9/5;
sowmy87	0:637fe5c74b2d	148	case 10:
sowmy87	0:637fe5c74b2d	149	return range10/5;
sowmy87	0:637fe5c74b2d	150	default:
sowmy87	0:637fe5c74b2d	151	return -1.0;
sowmy87	0:637fe5c74b2d	152	}
sowmy87	0:637fe5c74b2d	153	}
sowmy87	0:637fe5c74b2d	154
sowmy87	0:637fe5c74b2d	155
sowmy87	0:637fe5c74b2d	156	void Robot::TickTick() {
sowmy87	0:637fe5c74b2d	157	actualHeading = (*compass).readBearing();
sowmy87	0:637fe5c74b2d	158	prevDeltaHeading=deltaHeading;
sowmy87	0:637fe5c74b2d	159	deltaHeading=(desiredHeading-actualHeading);
sowmy87	0:637fe5c74b2d	160	if (deltaHeading<-1800)deltaHeading+=3600;
sowmy87	0:637fe5c74b2d	161	pid->setProcessValue(deltaHeading);
sowmy87	0:637fe5c74b2d	162	float delta=pid->compute();//*((float)desiredSpeed/(MAX_ROBOT_SPEED));
sowmy87	0:637fe5c74b2d	163	// pcr.printf("Desired Heading = %i,\t Actual Heading = %i,\t Delta Heading = %i\n\r",desiredHeading/10, actualHeading/10, deltaHeading/10);
sowmy87	0:637fe5c74b2d	164	// pcr.printf("SPD= %i, Delta = %i, \tadjustment = %i\n\r",desiredSpeed, deltaHeading/10,(int)delta);
sowmy87	1:8fa2ee8c005e	165	if (desiredSpeed)
sowmy87	1:8fa2ee8c005e	166	SetSpeed(desiredSpeed-(int)delta, desiredSpeed+(int)delta);
sowmy87	0:637fe5c74b2d	167
sowmy87	0:637fe5c74b2d	168	}
sowmy87	0:637fe5c74b2d	169
sowmy87	0:637fe5c74b2d	170	void Robot::TackTack() {
sowmy87	0:637fe5c74b2d	171	range[servoPosition*5+readingNumber] = sensor.read();
sowmy87	1:8fa2ee8c005e	172	// pcr.printf("range= %f\n\r",range[servoPosition*5+readingNumber]);
sowmy87	0:637fe5c74b2d	173	range0=range[0+0]+range[0+1]+range[0+2]+range[0+3]+range[0+4];
sowmy87	0:637fe5c74b2d	174	range1=range[5+0]+range[5+1]+range[5+2]+range[5+3]+range[5+4];
sowmy87	0:637fe5c74b2d	175	range2=range[10+0]+range[10+1]+range[10+2]+range[10+3]+range[10+4];
sowmy87	0:637fe5c74b2d	176	range3=range[15+0]+range[15+1]+range[15+2]+range[15+3]+range[15+4];
sowmy87	0:637fe5c74b2d	177	range4=range[20+0]+range[20+1]+range[20+2]+range[20+3]+range[20+4];
sowmy87	0:637fe5c74b2d	178	range5=range[25+0]+range[25+1]+range[25+2]+range[25+3]+range[25+4];
sowmy87	0:637fe5c74b2d	179	range6=range[30+0]+range[30+1]+range[30+2]+range[30+3]+range[30+4];
sowmy87	0:637fe5c74b2d	180	range7=range[35+0]+range[35+1]+range[35+2]+range[35+3]+range[35+4];
sowmy87	0:637fe5c74b2d	181	range8=range[40+0]+range[40+1]+range[40+2]+range[40+3]+range[40+4];
sowmy87	0:637fe5c74b2d	182	range9=range[45+0]+range[45+1]+range[45+2]+range[45+3]+range[45+4];
sowmy87	0:637fe5c74b2d	183	range10=range[50+0]+range[50+1]+range[50+2]+range[50+3]+range[50+4];
sowmy87	0:637fe5c74b2d	184
sowmy87	0:637fe5c74b2d	185	wait(SERVO_RATE/2);
sowmy87	0:637fe5c74b2d	186
sowmy87	0:637fe5c74b2d	187
sowmy87	0:637fe5c74b2d	188	switch (isFullScan) {
sowmy87	0:637fe5c74b2d	189	case 0:
sowmy87	0:637fe5c74b2d	190	if (servoDirection==1&&servoPosition>=10) {
sowmy87	0:637fe5c74b2d	191	readingNumber++;
sowmy87	0:637fe5c74b2d	192	servoDirection=-1;
sowmy87	0:637fe5c74b2d	193	}
sowmy87	0:637fe5c74b2d	194	if (servoDirection==-1&&servoPosition<=0) {
sowmy87	0:637fe5c74b2d	195	readingNumber++;
sowmy87	0:637fe5c74b2d	196	servoDirection=1;
sowmy87	0:637fe5c74b2d	197	}
sowmy87	0:637fe5c74b2d	198	break;
sowmy87	0:637fe5c74b2d	199	case 1:
sowmy87	0:637fe5c74b2d	200	default:
sowmy87	0:637fe5c74b2d	201	if (servoDirection==1&&servoPosition>=8) {
sowmy87	0:637fe5c74b2d	202	readingNumber++;
sowmy87	0:637fe5c74b2d	203	servoDirection=-1;
sowmy87	0:637fe5c74b2d	204	}
sowmy87	0:637fe5c74b2d	205	if (servoDirection==-1&&servoPosition<=3) {
sowmy87	0:637fe5c74b2d	206	readingNumber++;
sowmy87	0:637fe5c74b2d	207	servoDirection=1;
sowmy87	0:637fe5c74b2d	208	}
sowmy87	0:637fe5c74b2d	209	}
sowmy87	0:637fe5c74b2d	210	servoPosition+=servoDirection;
sowmy87	0:637fe5c74b2d	211	// pcr.printf("range= %f\n\r",servoPosition);
sowmy87	0:637fe5c74b2d	212	*servo=(float)servoPosition/10;
sowmy87	0:637fe5c74b2d	213	readingNumber%=5;
sowmy87	0:637fe5c74b2d	214
sowmy87	0:637fe5c74b2d	215	}
sowmy87	0:637fe5c74b2d	216
sowmy87	0:637fe5c74b2d	217	void Robot::FlashLEDs() {
sowmy87	0:637fe5c74b2d	218	if (abs(deltaHeading)<100) *led2=1;
sowmy87	0:637fe5c74b2d	219	else *led2=0;
sowmy87	0:637fe5c74b2d	220
sowmy87	0:637fe5c74b2d	221	if (deltaHeading>50) *led1=1;
sowmy87	0:637fe5c74b2d	222	else *led1=0;
sowmy87	0:637fe5c74b2d	223
sowmy87	0:637fe5c74b2d	224	if (deltaHeading<-50) *led3=1;
sowmy87	0:637fe5c74b2d	225	else *led3=0;
sowmy87	0:637fe5c74b2d	226	}
sowmy87	0:637fe5c74b2d	227
sowmy87	0:637fe5c74b2d	228
sowmy87	0:637fe5c74b2d	229	#endif

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	16 Dec 2010
Imports:	6
Forks:	0
Commits:	4
Dependents:	0
Dependencies:	0
Followers:	1

Robot.cpp@2:7146fc352133, 2010-12-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning