MacroRat
Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 13:2032db00f168
- Parent:
- 12:5790e56a056f
- Child:
- 14:9e7bb03ddccb
diff -r 5790e56a056f -r 2032db00f168 main.cpp
--- a/main.cpp Fri May 12 23:25:07 2017 +0000
+++ b/main.cpp Sat May 13 02:40:49 2017 +0000
@@ -17,46 +17,209 @@
*/
// Constants for when HIGH_PWM_VOLTAGE = 0.1
-#define IP_CONSTANT 15
+#define IP_CONSTANT 13
#define II_CONSTANT 0
-#define ID_CONSTANT 0
+#define ID_CONSTANT 1.
+
+const int desiredCountR = 1300;
+const int desiredCountL = 1400;
+
+void turnLeft(){
+ 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 - desiredCountL;
+ double desiredCount1 = initialCount1 + desiredCountL;
+
+ 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;
+ error1 = count1 - desiredCount1;
+
+ 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;
+ }
+ }
+
+ right_motor.brake();
+ left_motor.brake();
+}
void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right
double speed0 = 0.15;
double speed1 = 0.15;
- double kp = 0.00001;
+ double kp = 0.01;
+
+ int counter = 0;
+
+ int initialCount0 = encoder0.getPulses();
+ int initialCount1 = encoder1.getPulses();
+
+ double desiredCount0 = initialCount0 + desiredCountR;
+ double desiredCount1 = initialCount1 - desiredCountR;
+
+ int count0 = initialCount0;
+ int count1 = initialCount1;
- double desiredCount0 = 1500;
- double desiredCount1 = -1500;
+ 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;
- double error0 = 10.1;
- double error1 = 10.1;
+ right_motor.move(speed0);
+ left_motor.move(speed1);
+ counter = 0;
+ }else{
+ counter++;
+ right_motor.brake();
+ left_motor.brake();
+ }
- //right_motor.move(speed0);
- //left_motor.move(speed1);
+ 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(){
+ double speed0 = 0.15;
+ double speed1 = 0.15;
+ double kp = 0.01;
+
+ int counter = 0;
int initialCount0 = encoder0.getPulses();
int initialCount1 = encoder1.getPulses();
- int count0 = encoder0.getPulses();
- int count1 = encoder1.getPulses();
+ double desiredCount0 = initialCount0 - 3000;
+ double desiredCount1 = initialCount1 + 2700;
+
+ 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;
+ error1 = count1 - desiredCount1;
- while(!(abs(error0) < 10) && !(abs(error1) < 10)){
+ speed0 = error0 * kp + speed0;
+ speed1 = error1 * kp + speed1;
- count0 = encoder0.getPulses() - initialCount0;
- count1 = encoder1.getPulses() - initialCount1;
+ 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();
+}
- error0 = count0 + desiredCount0; // moves forward
- error1 = count1 + desiredCount1; // moves backwards
+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();
- speed0 = error0 * kp + speed0;
- speed1 = error1 * kp + speed1;
+ 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){
- right_motor.move(speed0);
- left_motor.move(speed1);
+ 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;
- 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.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();
@@ -121,10 +284,6 @@
left_motor.brake();
right_motor.brake();
- while( 1 )
- {
-
- }
}
void printDipFlag() {
@@ -194,10 +353,11 @@
// 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->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
@@ -227,9 +387,27 @@
while (1) {
- //moveForwardUntilWallIr();
- turnRight();
+ // moveForwardUntilWallIr();
+ // wait(2);
+ //turnRight180();
+ wait(0.5);
+ turnLeft180();
+ wait(1);
+ //turnRight180();
wait(1);
+ //turnLeft();
+
+ // 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) ) ;