Squirrels have fuzzy tails.
0 Members and 1 Guest are viewing this topic.
#include <PololuWheelEncoders.h>// sensorsunsigned char front_left_ir = 6;unsigned char front_right_ir = 5;unsigned char back_ir = 4;// right encoder on 2,3// left encoder on 18, 19 (analog 4,5)PololuWheelEncoders enc;// left motorunsigned char left_pwm = 10;unsigned char left_pin1 = 7;unsigned char left_pin2 = 8;// right motorunsigned char right_pwm = 9;unsigned char right_pin1 = 12;unsigned char right_pin2 = 13;// variablesboolean objR = false;boolean objL = false;boolean objB = false;float lspeed = 0;float rspeed = 0;float lspd = 0;float rspd = 0;void setup() { enc.init(2,3,18,19); pinMode(front_left_ir, INPUT); pinMode(front_right_ir, INPUT); pinMode(back_ir, INPUT); pinMode(left_pin1, OUTPUT); pinMode(left_pin2, OUTPUT); pinMode(right_pin1, OUTPUT); pinMode(right_pin2, OUTPUT); Serial.begin(9600); setSpd(1, 0.5); setSpd(2, 0.5);}void loop() { // max on both motors is 15 ticks/read digitalWrite(left_pin1, HIGH); digitalWrite(left_pin2, LOW); digitalWrite(right_pin1, HIGH); digitalWrite(right_pin2, LOW); analogWrite(left_pwm, 255 * lspeed); analogWrite(right_pwm, 255 * rspeed); clamp(1, lspd); clamp(2, rspd);}void setSpd(unsigned char motor, float spd) { switch(motor) { case 1: lspd = lspeed = spd; break; case 2: rspd = rspeed = spd; break; } }int readRate(unsigned char motor) { switch(motor){ case 1: enc.getCountsAndResetM1(); delay(100); return enc.getCountsAndResetM1(); case 2: enc.getCountsAndResetM1(); delay(100); return enc.getCountsAndResetM1(); }}void clamp(unsigned char motor, float percent) { int wantedSpeed = (int)( percent * 15.0); int realSpeed = readRate(motor); if (wantedSpeed - realSpeed < 0) switch(motor){ case 1: lspeed += 0.1; break; case 2: rspeed += 0.1; break; } else if (wantedSpeed - realSpeed > 0) switch(motor){ case 1: lspeed -= 0.1; break; case 2: rspeed -= 0.1; break; }}void scan() { objR = !(boolean) digitalRead(front_right_ir); objL = !(boolean) digitalRead(front_left_ir); objB = !(boolean) digitalRead(back_ir);}
My guess is that there's something wrong with my code: (below)
#include <PololuWheelEncoders.h>// sensorsunsigned char front_left_ir = 6;unsigned char front_right_ir = 5;unsigned char back_ir = 4;// right encoder on 2,3// left encoder on 18, 19 (analog 4,5)PololuWheelEncoders enc;// left motorunsigned char left_pwm = 10;unsigned char left_pin1 = 7;unsigned char left_pin2 = 8;// right motorunsigned char right_pwm = 9;unsigned char right_pin1 = 12;unsigned char right_pin2 = 13;// variables// if theres object at Right, Left, and Backboolean objR = false;boolean objL = false;boolean objB = false;// *speed is actual speed, [0, 1.0]// *spd is desired speed, [0, 1.0]float lspeed = 0;float rspeed = 0;float lspd = 0;float rspd = 0;void setup() { // init the encoders enc.init(2,3,18,19); // setup IR sensors pinMode(front_left_ir, INPUT); pinMode(front_right_ir, INPUT); pinMode(back_ir, INPUT); // setup motor pins pinMode(left_pin1, OUTPUT); pinMode(left_pin2, OUTPUT); pinMode(right_pin1, OUTPUT); pinMode(right_pin2, OUTPUT); // serial Serial.begin(9600); // test with each motor at half speed setSpd(1, 0.5); setSpd(2, 0.5);}void loop() { // max on both motors is 15 ticks/read // set left motor to move forward digitalWrite(left_pin1, HIGH); digitalWrite(left_pin2, LOW); // set right motor to move forward; digitalWrite(right_pin1, HIGH); digitalWrite(right_pin2, LOW); // use pwm to control speed, (l || r)speed is between 0 and 1.0 analogWrite(left_pwm, 255 * lspeed); analogWrite(right_pwm, 255 * rspeed); // adjust the pwm so that the speed is the same on left and right wheels clamp(1, lspd); clamp(2, rspd);}void setSpd(unsigned char motor, float spd) { // set the motor's desired speed and actual speed to a given value switch(motor) { case 1: lspd = lspeed = spd; break; case 2: rspd = rspeed = spd; break; } }int readRate(unsigned char motor) {// count the number of ticks the encoder moves in 100ms switch(motor){ case 1: enc.getCountsAndResetM1(); // reset the count delay(100); // wait 100 ms return enc.getCountsAndResetM1(); // get the value case 2: enc.getCountsAndResetM1(); // reset the count delay(100); // wait 100 ms return enc.getCountsAndResetM1(); // get the value }}void clamp(unsigned char motor, float percent) { // find the desired speed in ticks/100ms, which is the percent * max(ticks/100ms) int wantedSpeed = (int)( percent * 15.0); // find the actual speed of the motor in ticks/100ms int realSpeed = readRate(motor); // if the realspeed is > the wanted speed if (wantedSpeed - realSpeed < 0) switch(motor){ case 1: lspeed -= 0.1; break; case 2: rspeed -= 0.1; break; } // if the realspeed is < the wanted speed else if (wantedSpeed - realSpeed > 0) switch(motor){ case 1: lspeed += 0.1; break; case 2: rspeed += 0.1; break; }}// unusedvoid scan() { objR = !(boolean) digitalRead(front_right_ir); objL = !(boolean) digitalRead(front_left_ir); objB = !(boolean) digitalRead(back_ir);}