MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 38:fe05f93009a2
- Parent:
- 37:3dcc95e9321c
- Child:
- 39:058fb32c24e0
--- a/main.cpp Sat May 27 03:37:24 2017 +0000
+++ b/main.cpp Sat May 27 21:29:55 2017 +0000
@@ -90,8 +90,10 @@
double speed1 = WHEEL_SPEED;
right_motor.move(speed0);
left_motor.move(speed1);
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverTwoReading = IRP_4.getSamples(100);
- while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num)) {
+ while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) && receiverOneReading < IRP_1.sensorAvg*4 && receiverFourReading < IRP_4.sensorAvg*4) {
//while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
@@ -111,6 +113,65 @@
left_motor.move( speed1-0.8*d );
right_motor.move( speed0+d );
}
+ prev = x;
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverFourReading = IRP_4.getSamples(100);
+ }
+
+ //pidOnEncoders();
+ //pidBrake();
+ right_motor.brake();
+ left_motor.brake();
+ return;
+}
+
+
+void moveForwardWallEncoder()
+{
+ int count0;
+ int count1;
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+ int initial1 = count1;
+ int initial0 = count0;
+ int diff = count0 - count1;
+ double kp = 0.00015;
+ double kd = 0.00019;
+ int prev = 0;
+
+ double speed0 = WHEEL_SPEED;
+ double speed1 = WHEEL_SPEED;
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+
+ double ir1 = IRP_1.getSamples(50);
+ double ir4 = IRP_4.getSamples(50);
+
+ if(ir1 > IRP_1.sensorAvg*4 || ir4 > IRP_2.sensorAvg*4){
+ return;
+ }
+
+ //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
+ //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){
+ while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
+ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+
+ count0 = encoder0.getPulses() - initial0;
+ count1 = encoder1.getPulses() - initial1;
+ int x = count0 - count1;
+ //double d = kp * x + kd * ( x - prev );
+ double kppart = kp * x;
+ double kdpart = kd * (x-prev);
+ double d = kppart + kdpart;
+
+ //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
+ if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
+ left_motor.move( speed1-0.8*d );
+ right_motor.move( speed0+d );
+ } else if( x > diff + 40 ) {
+ left_motor.move( speed1-0.8*d );
+ right_motor.move( speed0+d );
+ }
// else
// {
// left_motor.brake();
@@ -126,136 +187,6 @@
return;
}
-
-void moveForwardWallEncoder()
-{
- int count0;
- int count1;
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
- int initial1 = count1;
- int initial0 = count0;
- int diff = count0 - count1;
- double kp = 0.00015;
- double kd = 0.00019;
- int prev = 0;
-
- double speed0 = WHEEL_SPEED;
- double speed1 = WHEEL_SPEED;
- right_motor.move(speed0);
- left_motor.move(speed1);
-
- double ir1 = IRP_1.getSamples(50);
- double ir4 = IRP_4.getSamples(50);
-
- if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4) {
- return;
- }
-
- //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
- //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){
- while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
- //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-
- count0 = encoder0.getPulses() - initial0;
- count1 = encoder1.getPulses() - initial1;
- int x = count0 - count1;
- //double d = kp * x + kd * ( x - prev );
- double kppart = kp * x;
- double kdpart = kd * (x-prev);
- double d = kppart + kdpart;
-
- //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
- if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
- left_motor.move( speed1-0.8*d );
- right_motor.move( speed0+d );
- } else if( x > diff + 40 ) {
- left_motor.move( speed1-0.8*d );
- right_motor.move( speed0+d );
- }
- // else
- // {
- // left_motor.brake();
- // right_motor.brake();
- // }
- prev = x;
- ir1 = IRP_1.getSamples(50);
- ir4 = IRP_4.getSamples(50);
- }
-
- //pidOnEncoders();
- //pidBrake();
- right_motor.brake();
- left_motor.brake();
- return;
-}
-
-/*
-void moveForwardUntilWallIr()
-{
- double currentError = 0;
- double previousError = 0;
- double derivError = 0;
- double sumError = 0;
-
- double HIGH_PWM_VOLTAGE = 0.1;
-
- double rightSpeed = 0.25;
- double leftSpeed = 0.23;
-
- float ir2 = IRP_2.getSamples( SAMPLE_NUM );
- float ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
- int count = encoder0.getPulses();
- while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
-
- if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
- //moveForwardWallEncoder();
- } else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005) { // left wall gone
- //moveForwardRightWall();
- } else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005) { // right wall gone
- //moveForwardLeftWall();
- } else {
- // will move forward using encoders only
- // if cell ahead doesn't have a wall on either one side or both sides
-
- int pulseCount = (encoder0.getPulses()-count) % 5600;
- if (pulseCount > 5400 && pulseCount < 5800) {
- blueLed.write(0);
- } else {
- blueLed.write(1);
- }
- sumError += currentError;
- currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ;
- derivError = currentError - previousError;
- double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
- if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
- rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
- leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
- } else { // r is faster than L. speed up l, slow down r
- rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
- leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
- }
-
- if (leftSpeed > 0.30) leftSpeed = 0.30;
- if (leftSpeed < 0) leftSpeed = 0;
- if (rightSpeed > 0.30) rightSpeed = 0.30;
- if (rightSpeed < 0) rightSpeed = 0;
-
- right_motor.forward(rightSpeed);
- left_motor.forward(leftSpeed);
-
- previousError = currentError;
-
- ir2 = IRP_2.getSamples( SAMPLE_NUM );
- ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
- }
- left_motor.brake();
- right_motor.brake();
- }
-}
-*/
void printDipFlag()
{
if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
@@ -375,7 +306,7 @@
receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
// serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
- if( receiverOneReading > IRP_1.sensorAvg * 3.8 || receiverFourReading > IRP_4.sensorAvg * 3.8) {
+ if( receiverOneReading > IRP_1.sensorAvg * 5.5 || receiverFourReading > IRP_4.sensorAvg * 5.5) {
break;
}
@@ -942,6 +873,22 @@
wait_ms(300);
turnRight();
nCellEncoderAndIR(4);
+ wait_ms(300);
+ turnLeft();
+ wait_ms(300);
+ nCellEncoderAndIR(1);
+ wait_ms(300);
+ turnLeft();
+ wait_ms(300);
+ nCellEncoderAndIR(3);
+ wait_ms(300);
+ turnRight();
+ wait_ms(300);
+ nCellEncoderAndIR(3);
+ wait_ms(300);
+ turnRight();
+ wait_ms(300);
+ nCellEncoderAndIR(3);
waitButton4();
// serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );
// double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;