Robot - | Mbed

Users » sowmy87 » Code » Robot

Robot.cpp@3:36c4d1dbed7e, 2010-12-16 (annotated)

Committer:: sowmy87
Date:: Thu Dec 16 09:46:30 2010 +0000
Revision:: 3:36c4d1dbed7e
Parent:: 2:7146fc352133

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	3:36c4d1dbed7e	119	deg%=360;
sowmy87	3:36c4d1dbed7e	120	desiredHeading=(desiredHeading+deg*10)%3600;
sowmy87	3:36c4d1dbed7e	121
sowmy87	2:7146fc352133	122
sowmy87	2:7146fc352133	123	}
sowmy87	2:7146fc352133	124
sowmy87	2:7146fc352133	125	void Robot::PivetLeft(int deg){
sowmy87	3:36c4d1dbed7e	126	deg%=360;
sowmy87	3:36c4d1dbed7e	127	desiredHeading=(desiredHeading-deg*10)%3600;
sowmy87	2:7146fc352133	128	}
sowmy87	2:7146fc352133	129
sowmy87	0:637fe5c74b2d	130	float Robot::GetRange(int position) {
sowmy87	0:637fe5c74b2d	131	switch (position) {
sowmy87	0:637fe5c74b2d	132	case 0:
sowmy87	0:637fe5c74b2d	133	return range0/5;
sowmy87	0:637fe5c74b2d	134	case 1:
sowmy87	0:637fe5c74b2d	135	return range1/5;
sowmy87	0:637fe5c74b2d	136	case 2:
sowmy87	0:637fe5c74b2d	137	return range2/5;
sowmy87	0:637fe5c74b2d	138	case 3:
sowmy87	0:637fe5c74b2d	139	return range3/5;
sowmy87	0:637fe5c74b2d	140	case 4:
sowmy87	0:637fe5c74b2d	141	return range4/5;
sowmy87	0:637fe5c74b2d	142	case 5:
sowmy87	0:637fe5c74b2d	143	return range5/5;
sowmy87	0:637fe5c74b2d	144	case 6:
sowmy87	0:637fe5c74b2d	145	return range6/5;
sowmy87	0:637fe5c74b2d	146	case 7:
sowmy87	0:637fe5c74b2d	147	return range7/5;
sowmy87	0:637fe5c74b2d	148	case 8:
sowmy87	0:637fe5c74b2d	149	return range8/5;
sowmy87	0:637fe5c74b2d	150	case 9:
sowmy87	0:637fe5c74b2d	151	return range9/5;
sowmy87	0:637fe5c74b2d	152	case 10:
sowmy87	0:637fe5c74b2d	153	return range10/5;
sowmy87	0:637fe5c74b2d	154	default:
sowmy87	0:637fe5c74b2d	155	return -1.0;
sowmy87	0:637fe5c74b2d	156	}
sowmy87	0:637fe5c74b2d	157	}
sowmy87	0:637fe5c74b2d	158
sowmy87	0:637fe5c74b2d	159
sowmy87	0:637fe5c74b2d	160	void Robot::TickTick() {
sowmy87	0:637fe5c74b2d	161	actualHeading = (*compass).readBearing();
sowmy87	0:637fe5c74b2d	162	deltaHeading=(desiredHeading-actualHeading);
sowmy87	0:637fe5c74b2d	163	if (deltaHeading<-1800)deltaHeading+=3600;
sowmy87	0:637fe5c74b2d	164	pid->setProcessValue(deltaHeading);
sowmy87	0:637fe5c74b2d	165	float delta=pid->compute();//*((float)desiredSpeed/(MAX_ROBOT_SPEED));
sowmy87	0:637fe5c74b2d	166	// pcr.printf("Desired Heading = %i,\t Actual Heading = %i,\t Delta Heading = %i\n\r",desiredHeading/10, actualHeading/10, deltaHeading/10);
sowmy87	0:637fe5c74b2d	167	// pcr.printf("SPD= %i, Delta = %i, \tadjustment = %i\n\r",desiredSpeed, deltaHeading/10,(int)delta);
sowmy87	1:8fa2ee8c005e	168	if (desiredSpeed)
sowmy87	1:8fa2ee8c005e	169	SetSpeed(desiredSpeed-(int)delta, desiredSpeed+(int)delta);
sowmy87	0:637fe5c74b2d	170
sowmy87	0:637fe5c74b2d	171	}
sowmy87	0:637fe5c74b2d	172
sowmy87	0:637fe5c74b2d	173	void Robot::TackTack() {
sowmy87	0:637fe5c74b2d	174	range[servoPosition*5+readingNumber] = sensor.read();
sowmy87	1:8fa2ee8c005e	175	// pcr.printf("range= %f\n\r",range[servoPosition*5+readingNumber]);
sowmy87	0:637fe5c74b2d	176	range0=range[0+0]+range[0+1]+range[0+2]+range[0+3]+range[0+4];
sowmy87	0:637fe5c74b2d	177	range1=range[5+0]+range[5+1]+range[5+2]+range[5+3]+range[5+4];
sowmy87	0:637fe5c74b2d	178	range2=range[10+0]+range[10+1]+range[10+2]+range[10+3]+range[10+4];
sowmy87	0:637fe5c74b2d	179	range3=range[15+0]+range[15+1]+range[15+2]+range[15+3]+range[15+4];
sowmy87	0:637fe5c74b2d	180	range4=range[20+0]+range[20+1]+range[20+2]+range[20+3]+range[20+4];
sowmy87	0:637fe5c74b2d	181	range5=range[25+0]+range[25+1]+range[25+2]+range[25+3]+range[25+4];
sowmy87	0:637fe5c74b2d	182	range6=range[30+0]+range[30+1]+range[30+2]+range[30+3]+range[30+4];
sowmy87	0:637fe5c74b2d	183	range7=range[35+0]+range[35+1]+range[35+2]+range[35+3]+range[35+4];
sowmy87	0:637fe5c74b2d	184	range8=range[40+0]+range[40+1]+range[40+2]+range[40+3]+range[40+4];
sowmy87	0:637fe5c74b2d	185	range9=range[45+0]+range[45+1]+range[45+2]+range[45+3]+range[45+4];
sowmy87	0:637fe5c74b2d	186	range10=range[50+0]+range[50+1]+range[50+2]+range[50+3]+range[50+4];
sowmy87	0:637fe5c74b2d	187
sowmy87	0:637fe5c74b2d	188	wait(SERVO_RATE/2);
sowmy87	0:637fe5c74b2d	189
sowmy87	0:637fe5c74b2d	190
sowmy87	0:637fe5c74b2d	191	switch (isFullScan) {
sowmy87	0:637fe5c74b2d	192	case 0:
sowmy87	0:637fe5c74b2d	193	if (servoDirection==1&&servoPosition>=10) {
sowmy87	0:637fe5c74b2d	194	readingNumber++;
sowmy87	0:637fe5c74b2d	195	servoDirection=-1;
sowmy87	0:637fe5c74b2d	196	}
sowmy87	0:637fe5c74b2d	197	if (servoDirection==-1&&servoPosition<=0) {
sowmy87	0:637fe5c74b2d	198	readingNumber++;
sowmy87	0:637fe5c74b2d	199	servoDirection=1;
sowmy87	0:637fe5c74b2d	200	}
sowmy87	0:637fe5c74b2d	201	break;
sowmy87	0:637fe5c74b2d	202	case 1:
sowmy87	0:637fe5c74b2d	203	default:
sowmy87	0:637fe5c74b2d	204	if (servoDirection==1&&servoPosition>=8) {
sowmy87	0:637fe5c74b2d	205	readingNumber++;
sowmy87	0:637fe5c74b2d	206	servoDirection=-1;
sowmy87	0:637fe5c74b2d	207	}
sowmy87	0:637fe5c74b2d	208	if (servoDirection==-1&&servoPosition<=3) {
sowmy87	0:637fe5c74b2d	209	readingNumber++;
sowmy87	0:637fe5c74b2d	210	servoDirection=1;
sowmy87	0:637fe5c74b2d	211	}
sowmy87	0:637fe5c74b2d	212	}
sowmy87	0:637fe5c74b2d	213	servoPosition+=servoDirection;
sowmy87	0:637fe5c74b2d	214	// pcr.printf("range= %f\n\r",servoPosition);
sowmy87	0:637fe5c74b2d	215	*servo=(float)servoPosition/10;
sowmy87	0:637fe5c74b2d	216	readingNumber%=5;
sowmy87	0:637fe5c74b2d	217
sowmy87	0:637fe5c74b2d	218	}
sowmy87	0:637fe5c74b2d	219
sowmy87	0:637fe5c74b2d	220	void Robot::FlashLEDs() {
sowmy87	0:637fe5c74b2d	221	if (abs(deltaHeading)<100) *led2=1;
sowmy87	0:637fe5c74b2d	222	else *led2=0;
sowmy87	0:637fe5c74b2d	223
sowmy87	0:637fe5c74b2d	224	if (deltaHeading>50) *led1=1;
sowmy87	0:637fe5c74b2d	225	else *led1=0;
sowmy87	0:637fe5c74b2d	226
sowmy87	0:637fe5c74b2d	227	if (deltaHeading<-50) *led3=1;
sowmy87	0:637fe5c74b2d	228	else *led3=0;
sowmy87	0:637fe5c74b2d	229	}
sowmy87	0:637fe5c74b2d	230
sowmy87	0:637fe5c74b2d	231
sowmy87	0:637fe5c74b2d	232	#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@3:36c4d1dbed7e, 2010-12-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning