MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 21:9a6cb07bdcb6
- Parent:
- 20:82836745332e
- Child:
- 22:681190ff98f0
diff -r 82836745332e -r 9a6cb07bdcb6 main.cpp
--- a/main.cpp Wed May 17 02:01:34 2017 +0000
+++ b/main.cpp Thu May 18 02:52:22 2017 +0000
@@ -16,9 +16,12 @@
*/
// Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 8.85
-#define II_CONSTANT 0.005
-#define ID_CONSTANT 3.15
+// #define IP_CONSTANT 8.85
+// #define II_CONSTANT 0.005
+// #define ID_CONSTANT 3.15
+#define IP_CONSTANT 7.85
+#define II_CONSTANT 0.095
+#define ID_CONSTANT 20.85
const int desiredCountR = 1400;
const int desiredCountL = 1475;
@@ -33,6 +36,7 @@
void pidOnEncoders();
+
void turnLeft()
{
double speed0 = 0.15;
@@ -265,6 +269,112 @@
}
}
+void pidBrake(){
+
+ int count0;
+ int count1;
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+ int initial0 = count0;
+ int initial1 = count1;
+ double kp = 0.00011;
+
+
+
+ int error = count0 - count1;
+
+ int counter = 0;
+ right_motor.move(0.7);
+ left_motor.move(0.7);
+
+ double speed0 = 0.7;
+ double speed1 = 0.7;
+
+ while(1)
+ {
+ if(abs(error) < 3){
+ right_motor.brake();
+ left_motor.brake();
+ counter++;
+ }else{
+ count0 = encoder0.getPulses() - initial0;
+ count1 = encoder1.getPulses() - initial1;
+ error = count0 - count1;
+ speed0 = -error*kp + speed0;
+ speed1 = error*kp + speed1;
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+
+ counter = 0;
+ }
+ if (counter > 10){
+ break;
+ }
+
+ }
+ return;
+}
+
+void moveForwardEncoder(){
+
+ 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 = 0.1;
+ double speed1 = 0.13;
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+
+
+ while((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005) && ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.25) ) {
+
+ //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;
+ }
+
+ //pidOnEncoders();
+ pidBrake();
+ //right_motor.brake();
+ //left_motor.brake();
+ return;
+}
+
void moveForwardUntilWallIr()
{
double currentError = 0;
@@ -274,53 +384,62 @@
double HIGH_PWM_VOLTAGE = 0.1;
- double rightSpeed = 0.1;
- double leftSpeed = 0.12;
+ double rightSpeed = 0.14;
+ double leftSpeed = 0.17;
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) {
- 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) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
- 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);
+ 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)) {
+ serial.printf("asdfasdf \n");
+ moveForwardEncoder();
+ }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) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+ 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 );
+
}
- 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;
+ //backward();
+ wait_ms(40);
+ //brake();
- ir2 = IRP_2.getSamples( SAMPLE_NUM );
- ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
+ left_motor.brake();
+ right_motor.brake();
}
-
- //backward();
- wait_ms(40);
- //brake();
-
- left_motor.brake();
- right_motor.brake();
}
void printDipFlag()
@@ -472,6 +591,8 @@
ir3 = IRP_3.getSamples( SAMPLE_NUM );
}
+
+
left_motor.brake();
right_motor.brake();
}
@@ -536,23 +657,34 @@
//left_motor.forward( 0.2 );
while (1) {
+
+ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+
// oneCellEncoderAndIR(5);
- // break;
- moveForwardCellEncoder(1);
- wait(0.5);
- handleTurns();
- wait(0.5);
- moveForwardCellEncoder(1);
- wait(0.5);
- handleTurns();
- break;
+ moveForwardUntilWallIr();
+ //moveForwardEncoder();
+ //serial.printf("ded \n");
+ //break;
+
+
+ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
+
+
+ //break;
+ // moveForwardCellEncoder(1);
+ // wait(0.5);
+ // handleTurns();
+ // wait(0.5);
+ // moveForwardCellEncoder(1);
+ // wait(0.5);
+ // handleTurns();
+ //break;
//pidOnEncoders();
// moveForwardUntilWallIr();
//serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
//serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
// double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
- // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
- // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
+ //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
//reading = Rec_4.read();
// serial.printf("reading: %f\n", reading);