MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 14:9e7bb03ddccb
- Parent:
- 13:2032db00f168
- Child:
- 15:b80555a4a8b9
--- a/main.cpp Sat May 13 02:40:49 2017 +0000
+++ b/main.cpp Sat May 13 23:49:02 2017 +0000
@@ -6,10 +6,9 @@
#include <stdlib.h>
#include "ITG3200.h"
-#include "stm32f4xx.h"
+#include "stm32f4xx.h"
#include "QEI.h"
-
/* Constants for when HIGH_PWM_VOLTAGE = 0.2
#define IP_CONSTANT 6
#define II_CONSTANT 0
@@ -17,229 +16,263 @@
*/
// Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 13
+#define IP_CONSTANT 12
#define II_CONSTANT 0
-#define ID_CONSTANT 1.
+#define ID_CONSTANT 2
+
+const int desiredCountR = 1400;
+const int desiredCountL = 1475;
-const int desiredCountR = 1300;
-const int desiredCountL = 1400;
+const int oneCellCount = 5300;
-void turnLeft(){
+void turnLeft()
+{
double speed0 = 0.15;
- double speed1 = 0.15;
- double kp = 0.01;
+ double speed1 = -0.15;
int counter = 0;
-
- int initialCount0 = encoder0.getPulses();
- int initialCount1 = encoder1.getPulses();
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
- double desiredCount0 = initialCount0 - desiredCountL;
- double desiredCount1 = initialCount1 + desiredCountL;
+ int desiredCount0 = initial0 - desiredCountL;
+ int desiredCount1 = initial1 + desiredCountL;
- int count0 = initialCount0;
- int count1 = initialCount1;
+ int count0 = initial0;
+ int count1 = initial1;
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
- while(1){
- if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- speed0 = error0 * kp + speed0;
- speed1 = error1 * kp + speed1;
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
- }else{
+ } else {
counter++;
right_motor.brake();
left_motor.brake();
}
- if (counter > 60){
+ if (counter > 60) {
break;
}
- }
+ }
right_motor.brake();
left_motor.brake();
}
-void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right
- double speed0 = 0.15;
+void turnRight()
+{
+ double speed0 = -0.15;
double speed1 = 0.15;
- double kp = 0.01;
int counter = 0;
-
- int initialCount0 = encoder0.getPulses();
- int initialCount1 = encoder1.getPulses();
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
- double desiredCount0 = initialCount0 + desiredCountR;
- double desiredCount1 = initialCount1 - desiredCountR;
+ int desiredCount0 = initial0 + desiredCountR;
+ int desiredCount1 = initial1 - desiredCountR;
- int count0 = initialCount0;
- int count1 = initialCount1;
+ int count0 = initial0;
+ int count1 = initial1;
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
- while(1){
- if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
- error0 = count0 - desiredCount0; // moves forward
- error1 = count1 - desiredCount1; // moves backwards
-
- speed0 = error0 * kp + speed0;
- speed1 = error1 * kp + speed1;
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
- }else{
+ } else {
counter++;
right_motor.brake();
left_motor.brake();
}
- if (counter > 60){
+ if (counter > 60) {
break;
}
-
- // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
- // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses());
}
right_motor.brake();
left_motor.brake();
}
-void turnLeft180(){
+void turnLeft180()
+{
double speed0 = 0.15;
- double speed1 = 0.15;
- double kp = 0.01;
+ double speed1 = -0.15;
int counter = 0;
-
- int initialCount0 = encoder0.getPulses();
- int initialCount1 = encoder1.getPulses();
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
- double desiredCount0 = initialCount0 - 3000;
- double desiredCount1 = initialCount1 + 2700;
+ int desiredCount0 = initial0 - desiredCountL*2;
+ int desiredCount1 = initial1 + desiredCountL*2;
- int count0 = initialCount0;
- int count1 = initialCount1;
+ int count0 = initial0;
+ int count1 = initial1;
double error0 = count0 - desiredCount0;
double error1 = count1 - desiredCount1;
- while(1){
- if(!(abs(error0) < 1) && !(abs(error1) < 1)){
+ while(1) {
+
+ if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
count0 = encoder0.getPulses();
count1 = encoder1.getPulses();
- error0 = count0 - desiredCount0;
- error1 = count1 - desiredCount1;
-
- speed0 = error0 * kp + speed0;
- speed1 = error1 * kp + speed1;
+ error0 = count0 - desiredCount0;
+ error1 = count1 - desiredCount1;
right_motor.move(speed0);
left_motor.move(speed1);
counter = 0;
- }else{
+ } else {
counter++;
right_motor.brake();
left_motor.brake();
}
- if (counter > 60){
+ if (counter > 60) {
break;
}
- }
-
- right_motor.brake();
- left_motor.brake();
-}
-
-void turnRight180(){
- double speed0 = 0.15;
- double speed1 = 0.15;
- double kp = 0.01;
-
- int counter = 0;
-
- int initialCount0 = encoder0.getPulses();
- int initialCount1 = encoder1.getPulses();
-
- double desiredCount0 = initialCount0 + desiredCountR*2;
- double desiredCount1 = initialCount1 - desiredCountR*2;
-
- int count0 = initialCount0;
- int count1 = initialCount1;
-
- 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; // moves forward
- error1 = count1 - desiredCount1; // moves backwards
-
- speed0 = error0 * kp + speed0;
- speed1 = error1 * kp + speed1;
-
- right_motor.move(speed0);
- left_motor.move(speed1);
- counter = 0;
- }else{
- counter++;
- right_motor.brake();
- left_motor.brake();
- }
-
- if (counter > 60){
- break;
- }
-
- // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1);
- // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses());
}
right_motor.brake();
left_motor.brake();
}
-void moveForwardUntilWallIr() {
+void turnRight180()
+{
+ double speed0 = -0.15;
+ double speed1 = 0.15;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ int desiredCount0 = initial0 + desiredCountR*2;
+ int desiredCount1 = initial1 - desiredCountR*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;
+ } else {
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
+
+ if (counter > 60) {
+ break;
+ }
+ }
+ right_motor.brake();
+ left_motor.brake();
+}
+
+void moveForwardOneCellEncoder(){ // 0 is left wheel!
+ double speed0 = 0.15;
+ double speed1 = 0.15;
+
+ int counter = 0;
+ int initial0 = encoder0.getPulses();
+ int initial1 = encoder1.getPulses();
+
+ double kpLeft = 0.000005;
+ double kpRight = 0.000005;
+
+ int desiredCount0 = initial0 + oneCellCount;
+ int desiredCount1 = initial1 + oneCellCount;
+
+ int count0 = initial0;
+ int count1 = initial1;
+
+ int error = 0;
+
+ while (1){
+ if (count0 < desiredCount0 && count1 < desiredCount1){
+ count0 = encoder0.getPulses();
+ count1 = encoder1.getPulses();
+
+ error = count0 - count1; // if error is positive, then left has moved more, so we want to decrease its speed and increase the right speed
+
+ if (error > 0){
+ speed0 = 0.15 - error*kpLeft;
+ speed1 = 0.15 + error*kpRight;
+ }
+ else if (error <= 0){
+ speed0 = 0.15 + error*kpLeft;
+ speed1 = 0.15 - error*kpRight;
+ }
+ // serial.printf("The error is %d \n", error);
+ // serial.printf("The Left speed is: %f and the right speed is: %f \n", speed0, speed1);
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ }
+ else {
+ break;
+ }
+ }
+ greenLed.write(1);
+ blueLed.write(0);
+ right_motor.brake();
+ left_motor.brake();
+}
+
+void moveForwardUntilWallIr()
+{
double currentError = 0;
double previousError = 0;
double derivError = 0;
double sumError = 0;
-
+
double HIGH_PWM_VOLTAGE = 0.1;
-
+
double rightSpeed = 0.1;
double leftSpeed = 0.1;
-
+
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;
@@ -248,77 +281,86 @@
} else {
blueLed.write(1);
}
-
-
+
+
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;
+ 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);
+ 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);
+ } 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);
+ right_motor.forward(rightSpeed);
left_motor.forward(leftSpeed);
-
+
previousError = currentError;
-
+
ir2 = IRP_2.getSamples( SAMPLE_NUM );
- ir3 = IRP_3.getSamples( SAMPLE_NUM );
-
+ ir3 = IRP_3.getSamples( SAMPLE_NUM );
+
}
//sensor1Turn = IR_Sensor1.read();
//sensor4Turn = IR_Sensor4.read();
-
+
//backward();
wait_ms(40);
//brake();
-
+
left_motor.brake();
right_motor.brake();
}
-void printDipFlag() {
+void printDipFlag()
+{
if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
}
-void enableButton1() {
+void enableButton1()
+{
dipFlags |= BUTTON1_FLAG;
printDipFlag();
}
-void enableButton2() {
+void enableButton2()
+{
dipFlags |= BUTTON2_FLAG;
printDipFlag();
}
-void enableButton3() {
+void enableButton3()
+{
dipFlags |= BUTTON3_FLAG;
printDipFlag();
}
-void enableButton4() {
+void enableButton4()
+{
dipFlags |= BUTTON4_FLAG;
printDipFlag();
}
-void disableButton1() {
+void disableButton1()
+{
dipFlags &= ~BUTTON1_FLAG;
printDipFlag();
}
-void disableButton2() {
+void disableButton2()
+{
dipFlags &= ~BUTTON2_FLAG;
printDipFlag();
}
-void disableButton3() {
+void disableButton3()
+{
dipFlags &= ~BUTTON3_FLAG;
printDipFlag();
}
-void disableButton4() {
+void disableButton4()
+{
dipFlags &= ~BUTTON4_FLAG;
printDipFlag();
}
@@ -339,9 +381,9 @@
// IR_4.write(1);
redLed.write(1);
- greenLed.write(1);
+ greenLed.write(0);
blueLed.write(1);
-
+
//left_motor.forward(0.1);
//right_motor.forward(0.1);
@@ -351,14 +393,14 @@
// 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
- */
-
+ /* 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
@@ -372,49 +414,36 @@
// 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);
-
while (1) {
+ moveForwardOneCellEncoder();
- // moveForwardUntilWallIr();
- // wait(2);
- //turnRight180();
- wait(0.5);
- turnLeft180();
- wait(1);
- //turnRight180();
- wait(1);
- //turnLeft();
+ // moveForwardUntilWallIr();
+ // wait(2);
+ // turnRight();
+ // wait(1);
+ // moveForwardOneCellEncoder();
+ // moveForwardUntilWallIr();
- // wait(1);
- // turnLeft();
- // wait(1);
- // turnRight();
- // wait(1);
- // turnRight();
- // turnRight180();
- // wait(1);
- // turnLeft180();
- // wait(1);
break;
//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",
+
+ //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);
// redLed.write(0);