Diff: Robot.cpp
- Revision:
- 0:637fe5c74b2d
- Child:
- 1:8fa2ee8c005e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp Thu Dec 16 09:25:37 2010 +0000
@@ -0,0 +1,205 @@
+#ifndef __ROBOT__C
+#define __ROBOT__C
+
+#include "Robot.h"
+
+SRF05 sensor(p24,p25);
+
+
+Serial pcr(USBTX, USBRX);
+
+Robot::Robot() {
+ led1 = new DigitalOut(LED1);
+ led2 = new DigitalOut(LED2);
+ led3 = new DigitalOut(LED3);
+
+ compass=new CMPS03(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
+ wait(1);
+ desiredHeading=compass->readBearing();
+
+ pid = new PID(1.0, 0.00, 0.01, PID_RATE);
+ pid->setInputLimits(-450,450);
+ pid->setOutputLimits(-MAX_ROBOT_SPEED,MAX_ROBOT_SPEED);
+ pid->setBias(0);
+ pid->setMode(AUTO_MODE);
+ pid->setSetPoint(0);
+
+ range= new float[55];
+ returnRange = new float[11];
+// sensor1=new SRF05 (p22, p23);
+// sensor=new SRF05 (p24, p25);
+
+ isFullScan=0;
+ readingNumber=0;
+
+ servo=new Servo (p21);
+ servo->calibrate(.0009, 180);
+ servoPosition=5;
+ servoDirection=1;
+ *servo=(float)servoPosition/10;
+ wait(1);
+
+
+
+ ticker1.attach(this, &Robot::TickTick, PID_RATE*2);
+ ticker2.attach(this, &Robot::TackTack, SERVO_RATE);
+ ticker3.attach(this, &Robot::FlashLEDs, 1);
+}
+Robot::~Robot() {
+ delete compass;
+ delete pid;
+// delete sensor;
+ delete range;
+ delete returnRange;
+ delete servo;
+}
+
+int Robot::GetDesiredHeading() {
+ return desiredHeading/10;
+}
+int Robot::GetActualHeading() {
+ return actualHeading/10;
+}
+int Robot::SetDediredHeading(int deg) {
+ deg%=360;
+ if (deg<0)deg=deg+360;
+ desiredHeading=deg*10;
+ return deg;
+}
+
+void Robot::SetSpeed(int spd) {
+ int tmp=0;
+ desiredSpeed=spd%MAX_ROBOT_SPEED;
+ if (desiredSpeed>0)tmp=desiredSpeed+50;
+ if (desiredSpeed<0)tmp=desiredSpeed-50;
+ base.SetPWM(tmp);
+}
+
+void Robot::SetSpeed(int lSpd, int rSpd) {
+ lSpd%=MAX_ROBOT_SPEED;
+ rSpd%=MAX_ROBOT_SPEED;
+ if (lSpd>0)lSpd+=50;
+ if (lSpd<0)lSpd-=50;
+ if (rSpd>0)rSpd+=50;
+ if (rSpd<0)rSpd-=50;
+ base.SetPWM(lSpd,rSpd);
+}
+void Robot::SetLeftSpeed(int spd) {
+ spd%=MAX_ROBOT_SPEED;
+ if (spd>0)spd+=50;
+ if (spd<0)spd-=50;
+ base.SetLeftPWM(spd);
+}
+
+void Robot::SetRightSpeed(int spd) {
+ spd%=MAX_ROBOT_SPEED;
+ if (spd>0)spd+=50;
+ if (spd<0)spd-=50;
+// pcr.printf("R:%i\t\t",spd);
+ base.SetRightPWM(spd);
+}
+
+float Robot::GetRange(int position) {
+ switch (position) {
+ case 0:
+ return range0/5;
+ case 1:
+ return range1/5;
+ case 2:
+ return range2/5;
+ case 3:
+ return range3/5;
+ case 4:
+ return range4/5;
+ case 5:
+ return range5/5;
+ case 6:
+ return range6/5;
+ case 7:
+ return range7/5;
+ case 8:
+ return range8/5;
+ case 9:
+ return range9/5;
+ case 10:
+ return range10/5;
+ default:
+ return -1.0;
+ }
+}
+
+
+void Robot::TickTick() {
+ actualHeading = (*compass).readBearing();
+ prevDeltaHeading=deltaHeading;
+ deltaHeading=(desiredHeading-actualHeading);
+ if (deltaHeading<-1800)deltaHeading+=3600;
+ pid->setProcessValue(deltaHeading);
+ float delta=pid->compute();//*((float)desiredSpeed/(MAX_ROBOT_SPEED));
+// pcr.printf("Desired Heading = %i,\t Actual Heading = %i,\t Delta Heading = %i\n\r",desiredHeading/10, actualHeading/10, deltaHeading/10);
+// pcr.printf("SPD= %i, Delta = %i, \tadjustment = %i\n\r",desiredSpeed, deltaHeading/10,(int)delta);
+
+ SetSpeed(desiredSpeed-(int)delta, desiredSpeed+(int)delta);
+
+}
+
+void Robot::TackTack() {
+ range[servoPosition*5+readingNumber] = sensor.read();
+ // pcr.printf("range= %f\n\r",range[servoPosition*5+readingNumber]);
+ range0=range[0+0]+range[0+1]+range[0+2]+range[0+3]+range[0+4];
+ range1=range[5+0]+range[5+1]+range[5+2]+range[5+3]+range[5+4];
+ range2=range[10+0]+range[10+1]+range[10+2]+range[10+3]+range[10+4];
+ range3=range[15+0]+range[15+1]+range[15+2]+range[15+3]+range[15+4];
+ range4=range[20+0]+range[20+1]+range[20+2]+range[20+3]+range[20+4];
+ range5=range[25+0]+range[25+1]+range[25+2]+range[25+3]+range[25+4];
+ range6=range[30+0]+range[30+1]+range[30+2]+range[30+3]+range[30+4];
+ range7=range[35+0]+range[35+1]+range[35+2]+range[35+3]+range[35+4];
+ range8=range[40+0]+range[40+1]+range[40+2]+range[40+3]+range[40+4];
+ range9=range[45+0]+range[45+1]+range[45+2]+range[45+3]+range[45+4];
+ range10=range[50+0]+range[50+1]+range[50+2]+range[50+3]+range[50+4];
+
+ wait(SERVO_RATE/2);
+
+
+ switch (isFullScan) {
+ case 0:
+ if (servoDirection==1&&servoPosition>=10) {
+ readingNumber++;
+ servoDirection=-1;
+ }
+ if (servoDirection==-1&&servoPosition<=0) {
+ readingNumber++;
+ servoDirection=1;
+ }
+ break;
+ case 1:
+ default:
+ if (servoDirection==1&&servoPosition>=8) {
+ readingNumber++;
+ servoDirection=-1;
+ }
+ if (servoDirection==-1&&servoPosition<=3) {
+ readingNumber++;
+ servoDirection=1;
+ }
+ }
+ servoPosition+=servoDirection;
+// pcr.printf("range= %f\n\r",servoPosition);
+ *servo=(float)servoPosition/10;
+ readingNumber%=5;
+
+}
+
+void Robot::FlashLEDs() {
+ if (abs(deltaHeading)<100) *led2=1;
+ else *led2=0;
+
+ if (deltaHeading>50) *led1=1;
+ else *led1=0;
+
+ if (deltaHeading<-50) *led3=1;
+ else *led3=0;
+}
+
+
+#endif
\ No newline at end of file
Sowmiya Sivanandham