MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 24:e7063765d6f0
- Parent:
- 23:690b0ca34ee9
- Child:
- 25:f827a8b7880e
diff -r 690b0ca34ee9 -r e7063765d6f0 main.cpp
--- a/main.cpp Sat May 20 09:11:08 2017 +0000
+++ b/main.cpp Sun May 21 01:04:26 2017 +0000
@@ -1,20 +1,20 @@
#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
@@ -22,87 +22,88 @@
#define IP_CONSTANT 13.5
#define II_CONSTANT 0.095
#define ID_CONSTANT 8.85
-
+
const int desiredCount180 = 2900;
const int desiredCountR = 1500;
const int desiredCountL = 1575;
-
+
const int oneCellCount = 5400;
-const int oneCellCountMomentum = 4800; // one cell count is actually approximately 5400, but this value is considering momentum!
-
+const int oneCellCountMomentum = 4400;//4800; // 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;
-//IRSAvg= >: 0.143701, 0.128285
+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 averageDivL = 23.0; //blue
-float initAverageL = 10.35;
-
-float averageDivR = 24; //red
-float initAverageR = 12.82; //4.5
-
-float averageDivUpper = 0.9;
-
+
+
+ float noWallR = 0.007;
+ float noWallL = 0.007;
+
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;
@@ -111,45 +112,45 @@
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;
@@ -158,46 +159,46 @@
right_motor.brake();
left_motor.brake();
}
-
+
if (counter > 60) {
break;
}
}
-
+
right_motor.brake();
left_motor.brake();
turnFlag = 0;
currDir += 1;
}
-
+
void turnLeft180()
{
double speed0 = 0.15;
double speed1 = -0.15;
-
+
int counter = 0;
int initial0 = encoder0.getPulses();
int initial1 = encoder1.getPulses();
-
+
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;
@@ -206,45 +207,45 @@
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;
@@ -253,7 +254,7 @@
right_motor.brake();
left_motor.brake();
}
-
+
if (counter > 60) {
break;
}
@@ -262,11 +263,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);
@@ -274,23 +275,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 < IRP_3.sensorAvg/averageDivR){
+ if (receiverThreeReading < ir3base){
// redLed.write(1);
// blueLed.write(0);
turnFlag |= RIGHT_FLAG;
}
- else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){
+ else if (receiverTwoReading < ir2base){
// redLed.write(0);
// blueLed.write(1);
turnFlag |= LEFT_FLAG;
}
pidOnEncoders();
}
-
+
left_motor.brake();
right_motor.brake();
}
-
+
void handleTurns(){
if (turnFlag == 0x1){
// moveForwardCellEncoder(0.3);
@@ -306,9 +307,9 @@
turnRight();
}
}
-
+
void pidBrake(){
-
+
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -316,18 +317,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){
@@ -340,22 +341,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();
@@ -366,16 +367,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)) {
+
+
+ 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( (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 ));
@@ -405,17 +406,17 @@
// }
prev = x;
}
-
+
//pidOnEncoders();
//pidBrake();
right_motor.brake();
left_motor.brake();
return;
}
-
-
+
+
void moveForwardWallEncoder(){
-
+
int count0;
int count1;
count0 = encoder0.getPulses();
@@ -426,15 +427,19 @@
double kp = 0.00015;
double kd = 0.00019;
int prev = 0;
-
-
-
- double speed0 = 0.10;
- double speed1 = 0.12;
+
+
+
+ double speed0 = 0.07;
+ double speed1 = 0.07;
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){
+ 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 ){
//serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
@@ -465,29 +470,29 @@
// }
prev = x;
}
-
+
//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;
-
+
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
@@ -500,7 +505,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);
@@ -508,7 +513,7 @@
blueLed.write(1);
}
sumError += currentError;
- currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ;
+ currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - ir2base) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - ir3base) ) ;
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
@@ -518,36 +523,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;
@@ -588,7 +593,7 @@
dipFlags &= ~BUTTON4_FLAG;
printDipFlag();
}
-
+
void pidOnEncoders()
{
int count0;
@@ -599,7 +604,7 @@
double kp = 0.00011;
double kd = 0.00014;
int prev = 0;
-
+
int counter = 0;
while(1)
{
@@ -633,159 +638,158 @@
break;
}
}
-
+
void nCellEncoderAndIR(double cellCount){
double currentError = 0;
double previousError = 0;
double derivError = 0;
double sumError = 0;
-
- 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.17);
- right_motor.forward(0.15);
-
- float receiverTwoReading = 0.0;
- float receiverThreeReading = 0.0;
-
+
+ double HIGH_PWM_VOLTAGE = 0.15;
+ double rightSpeed = 0.095;
+ double leftSpeed = 0.115;
+
+
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
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);
+
int state = 0;
-
-
-
-
- while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
- receiverTwoReading = IRP_2.getSamples(100);
- receiverThreeReading = IRP_3.getSamples(100);
-
- if ((IRP_1.getSamples(100) > IRP_1.sensorAvg*0.1) || (IRP_4.getSamples(100) > IRP_4.sensorAvg*0.1) ){
- // almost to the end
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(1);
- moveForwardWallEncoder();
- break;
-
- }else if( (receiverThreeReading < IRP_3.sensorAvg/(averageDivR*0.8)) && (receiverTwoReading < IRP_2.sensorAvg/(averageDivL*0.8)) ){
- // both sides gone
- redLed.write(1);
- greenLed.write(1);
- blueLed.write(1);
- 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) - (0.6*IRP_2.sensorAvg/initAverageL);
- break;
- }
- case(2):{// blue
- currentError = (0.8*IRP_3.sensorAvg/initAverageL) - (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;
- }
- }
-
-
-
-
- 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){
+ ir3base = IRP_3.sensorAvg;
+ }
+ if(IRP_2.sensorAvg > noWallL){
+ ir2base = IRP_2.sensorAvg;
}
+ 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
@@ -799,29 +803,44 @@
// 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);
+ //turnRight180();
+ //wait_ms(1500);
while (1) {
//wait_ms(1500);
//turnRight();
//wait_ms(1500);
//turnLeft();
- nCellEncoderAndIR(4);
- wait_ms(2000);
+ nCellEncoderAndIR(1);
+ wait_ms(500);
// turnRight();
// wait_ms(500);
// nCellEncoderAndIR(1);
@@ -843,9 +862,9 @@
// nCellEncoderAndIR(5);
// break;
// turnRight180();
-
-
-
+
+
+
// int number = rand() % 4 + 1;
// switch(number){
// case(1):{//turn right
@@ -857,7 +876,7 @@
// break;
// }
// case(3):{// keep going
-
+
// break;
// }
// case(4):{// turnaround
@@ -868,27 +887,27 @@
// 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;
-
-
-
+
+
+
/*
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);
@@ -897,20 +916,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);
@@ -926,8 +945,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