MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 25:f827a8b7880e
- Parent:
- 24:e7063765d6f0
- Child:
- 26:d20f1adac2d3
--- a/main.cpp Sun May 21 01:04:26 2017 +0000
+++ b/main.cpp Sun May 21 08:52:43 2017 +0000
@@ -1,109 +1,125 @@
#include "mbed.h"
-
+
#include "irpair.h"
#include "main.h"
#include "motor.h"
-
+
#include <stdlib.h>
#include "ITG3200.h"
#include "stm32f4xx.h"
#include "QEI.h"
-
+
/* Constants for when HIGH_PWM_VOLTAGE = 0.2
#define IP_CONSTANT 6
#define II_CONSTANT 0
#define ID_CONSTANT 1
*/
-
+
// 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 13.5
+#define IP_CONSTANT 8.5
#define II_CONSTANT 0.095
-#define ID_CONSTANT 8.85
-
-const int desiredCount180 = 2900;
-const int desiredCountR = 1500;
-const int desiredCountL = 1575;
-
+#define ID_CONSTANT 6.85
+
+const int desiredCount180 = 2850;
+const int desiredCountR = 1575;
+const int desiredCountL = 1650;
+
const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4400;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
-
+const int oneCellCountMomentum = 4700; // one cell count is actually approximately 5400, but this value is considering momentum!
+
float receiverOneReading = 0.0;
float receiverTwoReading = 0.0;
float receiverThreeReading = 0.0;
float receiverFourReading = 0.0;
-float ir1base = 0.0;
-float ir2base = 0.0;
-
-float ir3base = 0.0;
-
-float ir4base = 0.0;
-
-//IRSAvg= >: 0.143701, 0.128285
-
-
-
-
-
-
-
- //facing wall ir2 and ir3
- //0.144562, 0.113971 in maze
-
- // normal hall ir2 and ir3
- //0.013665, 0.010889 in maze
-
- //0.014462, 0.009138
- // 0.013888, 0.010530
-
-
- //no walls ir2 and ir3
- //0.003265, 0.002904 in maze9
-
- //0.003162, 0.003123
- //0.003795,
-
-//0.005297, 0.007064
-
-
- float noWallR = 0.007;
- float noWallL = 0.007;
-
+float initAverageL = 8.28;
+float averageDivL = 29.5; //blue
+float initAverageR = 8.75; //4.5
+float averageDivR = 29.5; //red
+float averageDivUpper = 0.9;
+
void pidOnEncoders();
-
-
+
+
void turnLeft()
{
double speed0 = 0.11;
double speed1 = -0.13;
-
+
int counter = 0;
int initial0 = encoder0.getPulses();
int initial1 = encoder1.getPulses();
-
+
int desiredCount0 = initial0 - desiredCountL;
int desiredCount1 = initial1 + desiredCountL;
-
+
int count0 = initial0;
int count1 = initial1;
-
+
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
-
-
+
+
while(1) {
-
+
if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
-
+
error0 = count0 - desiredCount0;
error1 = count1 - desiredCount1;
-
+
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+ turnFlag = 0; // zeroing out the flags!
+ currDir -= 1;
+}
+
+void turnRight()
+{
+ double speed0 = -0.11;
+ double speed1 = 0.13;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 + desiredCountR;
+ int desiredCount1 = initial1 - desiredCountR;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ double error0 = count0 - desiredCount0;
+ double error1 = count1 - desiredCount1;
+
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
+
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
@@ -112,45 +128,46 @@
right_motor.brake();
left_motor.brake();
}
-
+
if (counter > 60) {
break;
}
}
-
+
right_motor.brake();
left_motor.brake();
- turnFlag = 0; // zeroing out the flags!
- currDir -= 1;
+ turnFlag = 0;
+ currDir += 1;
}
-
-void turnRight()
+
+void turnLeft180()
{
- double speed0 = -0.11;
- double speed1 = 0.13;
-
+ double speed0 = 0.15;
+ double speed1 = -0.15;
+
int counter = 0;
int initial0 = encoder0.getPulses();
int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 + desiredCountR;
- int desiredCount1 = initial1 - desiredCountR;
-
+
+ int desiredCount0 = initial0 - desiredCountL*2;
+ int desiredCount1 = initial1 + desiredCountL*2;
+
int count0 = initial0;
int count1 = initial1;
-
+
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
-
+
+
while(1) {
-
+
if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
-
+
error0 = count0 - desiredCount0;
error1 = count1 - desiredCount1;
-
+
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
@@ -159,46 +176,45 @@
right_motor.brake();
left_motor.brake();
}
-
+
if (counter > 60) {
break;
}
}
-
+
right_motor.brake();
left_motor.brake();
- turnFlag = 0;
- currDir += 1;
+ currDir -= 2;
}
-
-void turnLeft180()
+
+void turnRight180()
{
- double speed0 = 0.15;
- double speed1 = -0.15;
-
+ double speed0 = -0.16;
+ double speed1 = 0.16;
+
int counter = 0;
int initial0 = encoder0.getPulses();
int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 - desiredCountL*2;
- int desiredCount1 = initial1 + desiredCountL*2;
-
+
+ int desiredCount0 = initial0 + desiredCount180;
+ int desiredCount1 = initial1 - desiredCount180;
+
int count0 = initial0;
int count1 = initial1;
-
+
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
-
-
+
+
while(1) {
-
+
if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
-
+
error0 = count0 - desiredCount0;
error1 = count1 - desiredCount1;
-
+
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
@@ -207,54 +223,7 @@
right_motor.brake();
left_motor.brake();
}
-
- if (counter > 60) {
- break;
- }
- }
-
- right_motor.brake();
- left_motor.brake();
- currDir -= 2;
-}
-
-void turnRight180()
-{
- double speed0 = -0.16;
- double speed1 = 0.16;
-
- int counter = 0;
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
- int desiredCount0 = initial0 + desiredCount180;
- int desiredCount1 = initial1 - desiredCount180;
-
- int count0 = initial0;
- int count1 = initial1;
-
- double error0 = count0 - desiredCount0;
- double error1 = count1 - desiredCount1;
-
-
- while(1) {
-
- if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
- count0 = encoder0.getPulses();
- count1 = encoder1.getPulses();
-
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- } else {
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
+
if (counter > 60) {
break;
}
@@ -263,11 +232,11 @@
left_motor.brake();
currDir += 2;
}
-
+
void moveForwardCellEncoder(double cellNum){
int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
-
+
left_motor.forward(0.125);
right_motor.forward(0.095);
wait_ms(1);
@@ -275,23 +244,23 @@
receiverTwoReading = IRP_2.getSamples(100);
receiverThreeReading = IRP_3.getSamples(100);
// serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
- if (receiverThreeReading < ir3base){
+ if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){
// redLed.write(1);
// blueLed.write(0);
turnFlag |= RIGHT_FLAG;
}
- else if (receiverTwoReading < ir2base){
+ else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){
// redLed.write(0);
// blueLed.write(1);
turnFlag |= LEFT_FLAG;
}
pidOnEncoders();
}
-
+
left_motor.brake();
right_motor.brake();
}
-
+
void handleTurns(){
if (turnFlag == 0x1){
// moveForwardCellEncoder(0.3);
@@ -307,9 +276,9 @@
turnRight();
}
}
-
+
void pidBrake(){
-
+
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -317,18 +286,18 @@
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){
@@ -341,22 +310,22 @@
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();
@@ -367,16 +336,16 @@
double kp = 0.00015;
double kd = 0.00019;
int prev = 0;
-
-
-
+
+
+
double speed0 = 0.10;
double speed1 = 0.12;
right_motor.move(speed0);
left_motor.move(speed1);
-
-
- while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){
+
+
+ while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
//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 ));
@@ -406,17 +375,17 @@
// }
prev = x;
}
-
+
//pidOnEncoders();
//pidBrake();
right_motor.brake();
left_motor.brake();
return;
}
-
-
+
+
void moveForwardWallEncoder(){
-
+
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -427,21 +396,24 @@
double kp = 0.00015;
double kd = 0.00019;
int prev = 0;
-
-
-
- double speed0 = 0.07;
- double speed1 = 0.07;
+
+
+
+ double speed0 = 0.11;
+ double speed1 = 0.13;
right_motor.move(speed0);
left_motor.move(speed1);
- if( IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){
+ float ir1 = IRP_1.getSamples(50);
+ float 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( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){
+ //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;
@@ -469,30 +441,32 @@
// 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.14;
- double leftSpeed = 0.17;
-
+
+ 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
@@ -505,7 +479,7 @@
}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);
@@ -513,7 +487,7 @@
blueLed.write(1);
}
sumError += currentError;
- currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - ir2base) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - ir3base) ) ;
+ 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
@@ -523,36 +497,36 @@
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 );
-
+
}
-
+
//backward();
//wait_ms(40);
//brake();
-
+
left_motor.brake();
right_motor.brake();
}
}
-
+
void printDipFlag()
{
if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
}
-
+
void enableButton1()
{
dipFlags |= BUTTON1_FLAG;
@@ -593,7 +567,7 @@
dipFlags &= ~BUTTON4_FLAG;
printDipFlag();
}
-
+
void pidOnEncoders()
{
int count0;
@@ -604,7 +578,7 @@
double kp = 0.00011;
double kd = 0.00014;
int prev = 0;
-
+
int counter = 0;
while(1)
{
@@ -638,158 +612,211 @@
break;
}
}
-
+
void nCellEncoderAndIR(double cellCount){
double currentError = 0;
double previousError = 0;
double derivError = 0;
double sumError = 0;
-
- double HIGH_PWM_VOLTAGE = 0.15;
- double rightSpeed = 0.095;
- double leftSpeed = 0.115;
-
-
- int initial0 = encoder0.getPulses();
- int initial1 = encoder1.getPulses();
-
+
+ double HIGH_PWM_VOLTAGE = 0.1;
+ double rightSpeed = 0.10;
+ double leftSpeed = 0.10;
+
+ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
+ int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
+
+ left_motor.forward(0.28);
+ right_motor.forward(0.25);
+
+ float receiverTwoReading = 0.0;
+ float receiverThreeReading = 0.0;
+
float ir2 = IRP_2.getSamples( SAMPLE_NUM );
float ir3 = IRP_3.getSamples( SAMPLE_NUM );
- float ir1 = IRP_1.getSamples(50);
- float ir4 = IRP_4.getSamples(50);
-
+
+ // float previr2 = ir2;
+ // float previr3 = ir3;
+
int state = 0;
- if(IRP_3.sensorAvg > noWallR){
- ir3base = IRP_3.sensorAvg;
- }
- if(IRP_2.sensorAvg > noWallL){
- ir2base = IRP_2.sensorAvg;
+
+ while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1 && receiverOneReading < IRP_1.sensorAvg*0.8 && receiverFourReading < IRP_4.sensorAvg*0.8){
+ receiverTwoReading = IRP_2.getSamples(100);
+ receiverThreeReading = IRP_3.getSamples(100);
+ // previr2 = receiverTwoReading;
+ // previr3 = receiverThreeReading;
+ receiverOneReading = IRP_1.getSamples(100);
+ receiverFourReading = IRP_4.getSamples(100);
+
+ //if ((receiverOneReading+receiverFourReading)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.15 ){
+ if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){
+ // almost to the end
+ right_motor.move(-0.15);
+ left_motor.move(-0.15);
+
+ wait_ms(150);
+ right_motor.brake();
+ left_motor.brake();
+
+
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(1);
+ wait_ms(200);
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(0);
+ wait_ms(200);
+
+
+
+ redLed.write(1);
+ greenLed.write(0);
+ blueLed.write(1);
+ wait_ms(200);
+ redLed.write(0);
+ greenLed.write(1);
+ blueLed.write(1);
+
+
+
+
+ //moveForwardWallEncoder();
+
+
+ return;
+
+ }else if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL)) ){
+ // both sides gone
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(1);
+ wait_ms(100);
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(0);
+ wait_ms(200);
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(0);
+ wait_ms(200);
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(0);
+ wait_ms(200);
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(0);
+ moveForwardEncoder();
+ }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
+ // RED RED RED RED RED
+ state = 1;
+ redLed.write(0);
+ greenLed.write(1);
+ blueLed.write(1);
+ }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone
+ // BLUE BLUE BLUE BLUE
+ state = 2;
+ redLed.write(1);
+ greenLed.write(1);
+ blueLed.write(0);
+ }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
+ // both walls there
+ state = 0;
+ redLed.write(1);
+ greenLed.write(0);
+ blueLed.write(1);
+ }
+
+ switch(state){
+ case(0):{ // both walls there
+ currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ break;
+ }
+ case(1):{// RED RED RED RED RED
+ currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
+ break;
+ }
+ case(2):{// blue
+ currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ break;
+ }
+ default:{
+ currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+ break;
+ }
+ }
+ //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
+
+
+
+
+
+ sumError += currentError;
+ 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);
+ pidOnEncoders();
+
+ previousError = currentError;
+ ir2 = IRP_2.getSamples( SAMPLE_NUM );
+ ir3 = IRP_3.getSamples( SAMPLE_NUM );
+
}
- for (int i = 0; i < cellCount; i++){
- while ( ((encoder0.getPulses()-initial0) <= oneCellCountMomentum && (encoder1.getPulses()-initial1) <= oneCellCountMomentum) && ir1 < IRP_1.sensorAvg*0.7 && ir4 < IRP_4.sensorAvg*0.7 ){
- ir2 = IRP_2.getSamples(50);
- ir3 = IRP_3.getSamples(50);
- ir1 = IRP_1.getSamples(50);
- ir4 = IRP_4.getSamples(50);
- if((ir3 < ir3base/4) && (ir2 < ir2base/4)){
- // both sides gone
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(1);
- wait_ms(100);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- wait_ms(200);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- moveForwardEncoder();
- }else if (ir3 < ir3base/4){// right wall gone
- // RED RED RED RED RED
- currentError = (ir3 - ir2base);
- redLed.write(0);
- greenLed.write(1);
- blueLed.write(1);
- }else if (ir2 < ir2base){// left wall gone
- // BLUE BLUE BLUE BLUE
- currentError = (ir3base - ir2);
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(0);
- }else{
- // both walls there
- currentError = (ir2 - ir2base) - (ir3 - ir3base);
- redLed.write(1);
- greenLed.write(0);
- blueLed.write(1);
- }
-
- sumError += currentError;
- 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);
- pidOnEncoders();
-
- previousError = currentError;
- ir2 = IRP_2.getSamples( SAMPLE_NUM );
- ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
- if(IRP_3.sensorAvg > noWallR){
- continue;
- }else if(ir3 > noWallR){
- ir3base = ir3;
- }
-
- if(IRP_2.sensorAvg > noWallL){
- continue;
- }else if(ir2 > noWallL){
- ir2base = ir2;
- }
- }
- }
-
left_motor.brake();
right_motor.brake();
return;
}
-
+
int main()
{
//Set highest bandwidth.
//gyro.setLpBandwidth(LPFBW_42HZ);
serial.baud(9600);
//serial.printf("The gyro's address is %s", gyro.getWhoAmI());
-
+
wait (0.1);
-
-
+
+
redLed.write(1);
greenLed.write(0);
blueLed.write(1);
-
+
//left_motor.forward(0.1);
//right_motor.forward(0.1);
-
+
// PA_1 is A of right
// PA_0 is B of right
// PA_5 is A of left
// PB_3 is B of left
//QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
// QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
-
+
// TODO: Setting all the registers and what not for Quadrature Encoders
/* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
*/
-
+
// set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
// of alternate function specified
// 4 modes sets AHB1ENR
@@ -803,44 +830,100 @@
// SMCR - encoder mode
// CR1 reenabales
// then read CNT reg of timer
-
-
+
+
dipButton1.rise(&enableButton1);
dipButton2.rise(&enableButton2);
dipButton3.rise(&enableButton3);
dipButton4.rise(&enableButton4);
-
+
dipButton1.fall(&disableButton1);
dipButton2.fall(&disableButton2);
dipButton3.fall(&disableButton3);
dipButton4.fall(&disableButton4);
- if(dipFlags == 0x1){
- turnRight180();
- wait_ms(1000);
- }else{
- turnRight();
- IRP_1.calibrateSensor();
- IRP_4.calibrateSensor();
- wait_ms(300);
- turnLeft();
- wait_ms(300);
- }
+ //right_motor.forward( 0.2 );
+ //left_motor.forward( 0.2 );
+ turnRight180();
+ wait_ms(1500);
-
-
- //right_motor.forward( 0.2 );
- //left_motor.forward( 0.2 );
- //turnRight180();
- //wait_ms(1500);
while (1) {
//wait_ms(1500);
//turnRight();
//wait_ms(1500);
//turnLeft();
+
+
+ // float ir2 = IRP_2.getSamples(100);
+ // float ir3 = IRP_3.getSamples(100);
+ // float ir1 = IRP_1.getSamples(100);
+ // float ir4 = IRP_4.getSamples(100);
+
+
+ // if( ir1 > IRP_1.sensorAvg*0.3 || ir4 > IRP_4.sensorAvg*0.3 ){
+ // // almost to the end
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(1);
+ // wait_ms(200);
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(0);
+ // wait_ms(200);
+ // redLed.write(1);
+ // greenLed.write(0);
+ // blueLed.write(1);
+ // wait_ms(200);
+ // redLed.write(0);
+ // greenLed.write(1);
+ // blueLed.write(1);
+
+ // }else if((ir3 < IRP_3.sensorAvg/(averageDivR)) && (ir2 < IRP_2.sensorAvg/(averageDivL)) ){
+ // // both sides gone
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(1);
+ // wait_ms(100);
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(0);
+ // wait_ms(200);
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(0);
+ // wait_ms(200);
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(0);
+ // wait_ms(200);
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(0);
+ // }else if (ir3 < IRP_3.sensorAvg/averageDivR){// right wall gone
+ // // RED RED RED RED RED
+ // redLed.write(0);
+ // greenLed.write(1);
+ // blueLed.write(1);
+ // }else if (ir2 < IRP_2.sensorAvg/averageDivL){// left wall gone
+ // // BLUE BLUE BLUE BLUE
+ // redLed.write(1);
+ // greenLed.write(1);
+ // blueLed.write(0);
+ // }else if ((ir2 > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (ir3 > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
+ // // both walls there
+ // redLed.write(1);
+ // greenLed.write(0);
+ // blueLed.write(1);
+ // }
+
+
+
+
nCellEncoderAndIR(1);
- wait_ms(500);
+ wait_ms(1000);
+
+
// turnRight();
// wait_ms(500);
// nCellEncoderAndIR(1);
@@ -862,9 +945,9 @@
// nCellEncoderAndIR(5);
// break;
// turnRight180();
-
-
-
+
+
+
// int number = rand() % 4 + 1;
// switch(number){
// case(1):{//turn right
@@ -876,7 +959,7 @@
// break;
// }
// case(3):{// keep going
-
+
// break;
// }
// case(4):{// turnaround
@@ -887,27 +970,34 @@
// break;
// }
// }
-
+
// float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
// float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
-
+
// float ir3 = IRP_2.getSamples(100)/initAverageL;
// float ir2 = IRP_3.getSamples(100)/initAverageR;
-
-
-
+ //serial.printf("%f, %f \n", IRP_1.sensorAvg, IRP_4.sensorAvg);
+ //serial.printf("%f, %f \n", IRP_2.sensorAvg, IRP_3.sensorAvg);
+ //break;
+
+ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
+ //serial.printf("IRS= >: %f, %f \r\n", IRP_1.getSamples(100), IRP_4.getSamples(100));
+
+
+
+
/*
counter2++;
counter3++;
ir2tot += IRP_2.getSamples(100);
ir3tot += IRP_3.getSamples(100);
-
-
+
+
ir2 = ir2tot/counter2;
ir3 = ir3tot/counter3;
-
+
serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
*/
//serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
@@ -916,20 +1006,20 @@
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
//serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
//serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
-
-
+
+
////////////////////////////////////////////////////////////////
-
+
//nCellEncoderAndIR(3);
//break;
-
+
//serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
-
-
+
+
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-
-
+
+
//break;
// moveForwardCellEncoder(1);
// wait(0.5);
@@ -945,8 +1035,8 @@
//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 );
-
+
//reading = Rec_4.read();
// serial.printf("reading: %f\n", reading);
}
-}
\ No newline at end of file
+}