Please don't be to critical of my codeing. I am new to all this and realize I need a lot of help/advise so here is my code for my Chip2 robot project. I have done all of this the best way i know how and have probably made a thousand mistakes but all I am asking for is some help correcting my mistakes.
#include <Adafruit_SSD1306.h> //#include <NewPing.h> #include <MD_DS1307.h> #include <Adafruit_GFX.h> #include <TFT_HX8357.h> // Hardware-specific library TFT_HX8357 tft = TFT_HX8357(); // Invoke custom library #define TFT_GREY 0x5AEB // New colour //#include <TimerOne.h> #include <Wire.h> #include <Servo.h> #include <Adafruit_Sensor.h> #include <Adafruit_LSM303_U.h> Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345); #define SCREEN_WIDTH 128 // OLED display width, in pixels #define SCREEN_HEIGHT 32 // OLED display height, in pixels // Declaration for an SSD1306 display connected to I2C (SDA, SCL pins) #define OLED_RESET 0 // Reset pin # (or -1 if sharing Arduino reset pin) Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); #define NUMFLAKES 10 // Number of snowflakes in the animation example #define LOGO_HEIGHT 16 #define LOGO_WIDTH 16 static const unsigned char PROGMEM logo_bmp[] = { B00000000, B11000000, B00000001, B11000000, B00000001, B11000000, B00000011, B11100000, B11110011, B11100000, B11111110, B11111000, B01111110, B11111111, B00110011, B10011111, B00011111, B11111100, B00001101, B01110000, B00011011, B10100000, B00111111, B11100000, B00111111, B11110000, B01111100, B11110000, B01110000, B01110000, B00000000, B00110000 }; #define RpwmPin 4 #define LpwmPin 8 #define RdirPin 49 #define LdirPin 48 #define ENC_INR 3 #define ENC_INL 2 #define trigPin1 10 #define echoPin1 13 #define trigPin2 44 #define echoPin2 47 #define BlueIrPin 28 // Pulse count from encoder volatile long encoderValueRnow = 0; volatile long encoderValueLnow = 0; volatile long encoderValueL = 1; volatile long encoderValueR = 1; //NewPing sonar(trigPin1, echoPin1); float duration1, distance1; //int iterations = 5; float duration2, distance2; int STOPM = 0; //stop motor float GOMR = 32.0; // GO motor right PWM speed float GOML = 30.0; // GO motor left PWM speed int Fdir = 0; // forward int Rdir = 1; // reverse int STOPDIS = 10; // INT TO SET STOP DISTANCE FROM OBJECT int Servo1Pos = 0; // variable to store the servo position int blueIrdetect = digitalRead(BlueIrPin); volatile long lastencoderValueR = 0; volatile long lastencoderValueL = 0; int R180 = 458 ; int L180 = 458 ; int InSpinR = 0; int InSpinL = 0; int setspeed = 0; int RPMinterval = 500; long previousMillis = 0; long currentMillis = 0; float rpmR = 0; float rpmL = 0; float LbehindDif =0; float LaheadDif = 0; float RbehindDif =0; float RaheadDif = 0; // Motor encoder output pulse per rotation (change as required) #define ENC_COUNT_REV 374 int Dheading = 108; int heading ; int justshyofheading; int justoverheading; int lookRightDistance; int lookLeftDistance; int atc ;// int to indicate "IN" aligntocompass function int htol ; // int to set heading vs Dheading tolarance int justshydelay; int justoverdelay; int hournow; int offset =20;// set the correction offset value int numalign = 0; int rwa =0;// number of right wheel reverse adjustments int rwaDiv=0;// set to adjust rwa to cmmoved values int lwa =0;// number of LEFT wheel reverse adjustments int lwaDiv=0;// set to adjust lwa to cmmoved values const char *dow2String(uint8_t code) { static const char *str[] = { "---", "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat" }; if (code > 7) code = 0; return(str[code]); } //int cmmoved; Servo myservo; // create servo object to control a servo int cmmoved=(encoderValueR / 10); void setup() { // put your setup code here, to run once: //Serial.begin (9600); // Setup initial values for RPM timer previousMillis = millis(); tft.init(); tft.setRotation(2); tft.fillScreen(TFT_WHITE); // initialize with the I2C addr 0x3C display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Clear the buffer. display.clearDisplay(); //Timer1.initialize(250000); //Timer1.attachInterrupt(adjustmotorspeed); //Serial.println("Magnetometer Test"); //Serial.println(""); /* Initialise the sensor */ if(!mag.begin()) { /* There was a problem detecting the LSM303 ... check your connections */ //Serial.println("Ooops, no LSM303 detected ... Check your wiring!"); while(1); } myservo.attach(9); // attaches the servo on pin 9 to the servo pinMode(RpwmPin, OUTPUT); pinMode(LpwmPin, OUTPUT); pinMode(RdirPin, OUTPUT); pinMode(LdirPin, OUTPUT); pinMode(trigPin1, OUTPUT); pinMode(echoPin1, INPUT); pinMode(trigPin2, OUTPUT); pinMode(echoPin2, INPUT); pinMode(BlueIrPin,INPUT); // Set encoder as input with internal pullup pinMode(ENC_INR, INPUT_PULLUP); pinMode(ENC_INL, INPUT_PULLUP); // Attach interrupt attachInterrupt(digitalPinToInterrupt(ENC_INR), updateEncoderR, RISING); attachInterrupt(digitalPinToInterrupt(ENC_INL), updateEncoderL, RISING); } void loop() { rwaDiv=(rwa / 10); lwaDiv=(lwa / 10); tft.println ("in void loop function" ); int volt = analogRead(A0);// read the input double voltage = map(volt,0,1023, 0, 2500)+ offset;// map 0-1023 to 0-2500 and add correction offset voltage /=100;// divide by 100 to get the decimal values // Update RPM value every 1/4 second currentMillis = millis(); if (currentMillis - previousMillis > RPMinterval) { previousMillis = currentMillis; // Calculate RPM of Motors rpmR = (float)(lastencoderValueR * 60.0 / ENC_COUNT_REV); rpmL = (float)(lastencoderValueL * 60.0 / ENC_COUNT_REV); } cmmoved=(encoderValueR / 10); tft.fillScreen(TFT_WHITE); tft.setCursor(0, 0, 2); tft.setTextColor(TFT_BLACK,TFT_WHITE); tft.setTextSize(1); tft.print(" Chip2 Debug "); tft.println(); tft.println("In Void Loop Function"); tft.print("Voltage: "); tft.print(voltage);//print the voltge tft.println("V"); tft.println(); //tft.print("left RPM behind diff = "); //tft.println(LbehindDif); //tft.print("right RPM behind diff = "); //tft.println(RbehindDif); if(rpmR<2.0){ GOMR++; } if(rpmL<2.0){ GOML++; } if(rpmR>2.0){ GOMR--; } if(rpmL>2.0){ GOML--; } tft.print("Right Motor RPM = "); tft.print(rpmR); tft.println(" RPM"); tft.print("Left Motor RPM = "); tft.print(rpmL); tft.println(" RPM"); tft.print("GOMR = "); tft.println(GOMR); tft.print("GOML = "); tft.println(GOML); tft.print("RIGHT wheel reverse adjustments = "); tft.println (rwa); tft.print("RIGHT wheel adjustments Div = "); tft.println (rwaDiv); tft.print("LEFT wheel reverse adjustments = "); tft.println (lwa); tft.print("LEFT wheel adjustments Div = "); tft.println (lwaDiv); tft.print("Encoder Value Right = "); tft.println(encoderValueR); tft.print("Encoder Value Left = "); tft.println(encoderValueL); tft.print("Desired heading "); tft.print(Dheading); tft.println(" "); tft.print("Compass heading "); tft.print(heading); tft.println(" "); tft.print("distance1 = "); tft.print(distance1); tft.println(" cm"); tft.print("distance2 = "); tft.print(distance2); tft.println(" cm"); tft.print("Chip moved "); tft.print(cmmoved); tft.println ( " cm"); // tft.print("Number of times through void aligh "); // tft.print(numalign); // tft.println(" "); lastencoderValueR=0; lastencoderValueL=0; RTC.readTime(); //////Serial.println( dow2String(RTC.calcDoW(RTC.yyyy, RTC.mm, RTC.dd))); //hournow=RTC.h; //if (hournow >= 12){ ////Serial.println ("afternoon"); ////Serial.println(hournow); //} //if (hournow <= 12){ ////Serial.println( "Morning "); //} // //Serial.print ("Chip2 moved "); // //Serial.print (cmmoved) ; // //Serial.println (" centimeters "); ////Serial.print("Encoder Value Right = "); ////Serial.println(encoderValueR); /* display.setTextSize(1); display.setTextColor(WHITE); display.setCursor(0,10); display.print("Desired heading "); display.println(Dheading); display.print("Compass heading "); display.println(heading); */ //display.print (RTC.h,DEC); //display.print (":"); //display.print (RTC.m,DEC); //display.print (":"); //display.print (RTC.s,DEC); //display.print (" "); //display.print( dow2String(RTC.calcDoW(RTC.yyyy, RTC.mm, RTC.dd))); /* display.print(encoderValueL); display.print (" "); display.println(encoderValueR); display.print("fmR "); display.println(GOMR); display.print("DH "); display.print(Dheading); display.print(" CH "); display.println(heading); */ // display.print("fmL "); // display.println(GOML); //display.fillTriangle(30, 15, 0, 60, 60, 60, WHITE); //display.display(); ///delay(2000); //display.clearDisplay(); int mi = 0; int mj = 0; heading = compassheading(mi,mj); sonar1read(); sonar2read(); chipmove(); } void sonar1read(){ tft.println("in sonar1read function "); if (InSpinR > 0) { lookforclearpath(); } if (InSpinL > 0) { lookforclearpath(); } digitalWrite(trigPin1, LOW); delayMicroseconds(2); digitalWrite(trigPin1, HIGH); delayMicroseconds(10); digitalWrite(trigPin1, LOW); // Measure the response from the HC-SR04 Echo Pin duration1 = pulseIn(echoPin1, HIGH); // Determine distance1 from duration1 // Use 343 metres per second as speed of sound distance1 = (duration1 / 2) * 0.0343; delay(100); } void sonar2read(){ tft.println("in sonar2read function"); if (InSpinR > 0) { lookforclearpath(); } if (InSpinL > 0) { lookforclearpath(); } digitalWrite(trigPin2, LOW); delayMicroseconds(2); digitalWrite(trigPin2, HIGH); delayMicroseconds(10); digitalWrite(trigPin2, LOW); // Measure the response from the HC-SR04 Echo Pin duration2 = pulseIn(echoPin2, HIGH); // Determine distance2 from duration2 // Use 343 metres per second as speed of sound distance2 = (duration2 / 2) * 0.0343; } void updateEncoderR() { // Increment value for each pulse from encoder encoderValueR++; lastencoderValueR++; } void updateEncoderL() { // Increment value for each pulse from encoder encoderValueL++; lastencoderValueL++; } void chipmove(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } tft.println("in chip move function" ); cmmoved=(encoderValueR / 10); if (cmmoved >= 160 + rwaDiv) { ckpt1();// at kitchen table } if (cmmoved >= 460 + rwaDiv) { ckpt2();// at coffee table } if (cmmoved >= 610+ rwaDiv ){ ckpt3();// at back door } if (cmmoved >= 910+ rwaDiv){ ckpt4(); //at tv } if (cmmoved >= 1143+ rwaDiv){ ckpt5();//at end of fireplace } if (cmmoved >= 1385+ rwaDiv) { ckpt6();//at hall door } if (cmmoved >= 1520+ rwaDiv) { ckpt7 ();// at radio room fireplace } if (cmmoved >= 1690+ rwaDiv){ ckpt8 ();//at radio room couch // GOMR=46; // speed up to climb on carpet //GOML=40; // speed up to climb on carpet } if (cmmoved >= 2030+ rwaDiv) { ckpt9 ();// at end of couch } if (cmmoved >= 2142+ rwaDiv) { ckpt10 ();//at roomba } if (cmmoved >= 2408+ rwaDiv) { ckpt11 ();//at kitchen door } if (cmmoved>= 2512+ rwaDiv) { ckpt12 (); //start line } htolset(); //Serial.print("htol = "); //Serial.println (htol); Servo1Pos=80; myservo.write(Servo1Pos); // tell servo to go to position in variable 'Servo1Pos' delay(15); if (htol > 1){ aligntocompass(); } if (distance1 > STOPDIS ) { bothmotorsgoforward(); } //if (Dheading == heading ) { // } // if (distance1 <=STOPDIS ){ // bothmotorsstop(); // } } void ckpt1(){ aligntocompass(); // Clear the buffer. // display.clearDisplay(); // display.setTextSize(1); //display.setTextColor(WHITE); //display.setCursor(0,10); // display.print("Desired heading "); //display.println(Dheading); //display.print("Compass heading "); //display.println(heading); Dheading = 15;// at kitchen table } void ckpt2(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 103; // at coffee table } void ckpt3(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 16; // at back door } void ckpt4(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 295; //at tv } void ckpt5(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 348; //at end of fireplace } void ckpt6(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 289; //at hall door } void ckpt7(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 245; // at radio room fireplace } void ckpt8(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 203; // at radio room couch } void ckpt9(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 140; } void ckpt10(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 205; } void ckpt11(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 110; } void ckpt12(){ encoderValueR=0; encoderValueL=0; rwaDiv=0; rwa=0; lwaDiv=0; lwa=0; } void bothmotorsgoforward(){ tft.println("Both motors go, forward function"); setspeed=1; analogWrite(RpwmPin, GOMR); analogWrite(LpwmPin, GOML); digitalWrite(RdirPin, Fdir); digitalWrite(LdirPin, Fdir); } void bothmotorsstop(){ tft.println("in void bothmotorsstopped function"); //Serial.print("Encoder Value Right = "); //Serial.println(encoderValueR); analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); // Clear the buffer. display.clearDisplay(); display.setTextSize(1); display.setTextColor(WHITE); display.setCursor(0,10); display.print("Desired heading "); display.println(Dheading); display.print("Compass heading "); display.println(heading); cmmoved=(encoderValueR / 10); lookforclearpath(); } void htolset(){ //tft.println("in htolset functioon"); if (Dheading < heading) { htol = (heading - Dheading); htol = htol * 1.0; } if (Dheading > heading) { htol = (Dheading - heading); htol = htol * 1.0; } atc = 1; } void aligntocompass(){ tft.println("in aligntocompass function "); numalign++; atc = 1; justshyofheading=(Dheading - heading); justoverheading=(heading - Dheading ); if (justshyofheading < 10){ if (justshyofheading < 0) { justshyofheading =justshyofheading - justshyofheading; } if (justoverheading < 10){ if (justoverheading < 0) { justoverheading = justoverheading - justoverheading; } } } if (justoverheading >= 1 ) { if (justoverheading <=6 && justoverheading >=0 ){ digitalWrite(LdirPin,Fdir); digitalWrite(RdirPin,Fdir); GOMR=GOMR+10; analogWrite(RpwmPin,GOMR); analogWrite(LpwmPin,GOML); delay(50); GOMR=GOMR-9; } if (justoverheading>=5){ justoverdelay = (justoverheading * 10); analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); // delay(2000); digitalWrite(LdirPin,Rdir); digitalWrite(RdirPin,Fdir); lwa++;// number of LEFT wheel adjustment analogWrite(RpwmPin,GOMR); analogWrite(LpwmPin,GOML); delay (justoverdelay); } } if (justshyofheading >= 1) { if (justshyofheading <=6 && justoverheading >=0){ digitalWrite(LdirPin,Fdir); digitalWrite(RdirPin,Fdir); GOML=GOML+10; analogWrite(RpwmPin,GOMR); analogWrite(LpwmPin,GOML); delay(50); GOML=GOML-9; } if(justshyofheading>=5){ justshydelay = (justshyofheading * 10); analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); // delay(2000); digitalWrite(LdirPin,Fdir); digitalWrite(RdirPin,Rdir); rwa++;// number of RIGHT wheel adjustment analogWrite(RpwmPin,GOMR); analogWrite(LpwmPin,GOML); delay(justshydelay); } } } void compassdisplay(){ } int compassheading(int x, int y){ tft.println("in compassheading function"); int result; /* Get a new sensor event */ sensors_event_t event; mag.getEvent(&event); float Pi = 3.14159; // Calculate the angle of the vector y,x int heading = (atan2(event.magnetic.y,event.magnetic.x) * 180) / Pi; //result = x * y + heading ; result = heading ; // Normalize to 0-360 if (heading < 0) { heading = 360 + heading; result = heading ; } ////Serial.print("Compass Heading: "); ////Serial.println(heading); ////Serial.print("Dheading = "); ////Serial.println (Dheading); delay(50); //delay (5000); return result; } void ccwspin(){ tft.println ("in ccwspin function "); analogWrite(RpwmPin,GOMR); digitalWrite(RdirPin,Fdir); analogWrite(LpwmPin,GOML); digitalWrite(LdirPin,Rdir); } void cwspin(){ tft.println ("in cwspin function "); analogWrite(RpwmPin,GOMR); digitalWrite(RdirPin,Rdir); analogWrite(LpwmPin,GOML); digitalWrite(LdirPin,Fdir); } void lookforclearpath(){ tft.println("in look for clear path function"); LookRight(); LookLeft(); if (lookLeftDistance>= lookRightDistance){ ccwspin(); } if (lookLeftDistance<= lookRightDistance){ cwspin(); } } void LookRight(){ tft.println("in look right function"); Servo1Pos=25; myservo.write(Servo1Pos); // tell servo to go to position in variable 'Servo1Pos' delay(250); sonar1read(); lookRightDistance=distance1; //Serial.print("lookRightDistance = "); //Serial.println(lookRightDistance); delay(1000); } void LookLeft(){ tft.println("in look left function"); Servo1Pos=165; myservo.write(Servo1Pos); delay (250); sonar1read(); lookLeftDistance=distance1; //Serial.print("lookLeftDistance = "); //Serial.println(lookLeftDistance); delay(1000); } void adjustmotorspeed(){ tft.println(" in adjustmotorspeed function"); if (encoderValueR < encoderValueL){ GOMR =(GOMR + 1); if (GOMR > 42){ GOMR = 32; } } if ( encoderValueR > encoderValueL){ GOMR = (GOMR - 1); if (GOMR < 25){ GOMR=32; } } }
but all I am asking for is some help correcting my mistakes.
Now that is a very tall order! ? ? ?
The first obvious mistakes are the following lines.
long previousMillis = 0; long currentMillis = 0;
The variables should be declared "unsigned long"! Not critical, but this doubles the amount of time until the next reset to zero milliseconds.
The variables should be declared "unsigned long"! Not critical, but this doubles the amount of time until the next reset to zero milliseconds.
Thank you , i have corrected them in my code.
int setspeed = 0; int RPMinterval = 500; unsigned long previousMillis = 0; unsigned long currentMillis = 0; float rpmR = 0; float rpmL = 0;
Perhaps it might be a good idea to attach the .ino file. Being able to collapse the functions until needed, makes easier reviewing than constantly scrolling up and down through the whole sketch!
Perhaps it might be a good idea to attach the .ino file. Being able to collapse the functions until needed, makes easier reviewing than constantly scrolling up and down through the whole sketch!
Okay let me try to do that.
I forgot to mention that doing math with different data types can lead to unexpected results, therefore you need to change, just for safety's sake.
int RPMinterval = 500; to
unsigned long RPMinterval = 500;
I forgot to mention that doing math with different data types can lead to unexpected results, therefore you need to change, just for safety's sake.
Thank you I have changed the code.
int setspeed = 0; unsigned long RPMinterval = 500; unsigned long previousMillis = 0; unsigned long currentMillis = 0; float rpmR = 0;
You have a lot of repetition between lines 430 and 509.
Perhaps you should consider one function and a passed variable like this
ckpt(103); //function call, replaces ckpt2 void ckpt(int hdg){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = hdg; // at coffee table }
Now you can get rid of ckpt2 to ckpt11 altogether. Leave ckpt1 and ckpt12 as they are!
Now we come to lines 347 to 384 (multiple if statements). This is a classic situation for "switch case" statements!
switch(cmmoved){ case (>= 2512 + rwaDiv): ckpt12(); //start line break; case (>= 2408 + rwaDiv): ckpt(110); //start line break; case (>= 2142 + rwaDiv): ckpt(205); //start line break;
... //you fill in the rest
}
NOTE that I have inversed the conditional, starting by checking for the highest value first and have included the function call shown above. This should make your code a little more compact.
Try it out and check that it works properly, before adopting it permanently!
ckpt(110); //start line break; case (>= 2142 + rwaDiv): ckpt(205); //start line break;
I thank you again for taking your time to help me.
i am a little confused about " ckpt(110) and ckpt(205) as they are not in my code.
please let me know where I made my mistake(s)
also i am getting a compile error
chip2_10-27-2019.ino: In function 'void chipmove()':
chip2_10-27-2019:350:11: error: expected primary-expression before '>=' token
case (>= 2512 + rwaDiv):
^
chip2_10-27-2019:353:11: error: expected primary-expression before '>=' token
case (>= 2408 + rwaDiv):
^
chip2_10-27-2019:354:15: error: 'ckpt' was not declared in this scope
ckpt(110); //start line
^
chip2_10-27-2019:356:11: error: expected primary-expression before '>=' token
case (>= 2142 + rwaDiv):
^
exit status 1
expected primary-expression before '>=' token
i am a little confused about " ckpt(110) and ckpt(205) as they are not in my code.
These values are taken from the following original code "Dheading" variable:
void ckpt10(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 205; } void ckpt11(){ if (heading==0){ analogWrite(RpwmPin, STOPM); analogWrite(LpwmPin, STOPM); delay(15000); } Dheading = 110; }
chip2_10-27-2019:354:15: error: 'ckpt' was not declared in this scope
ckpt(110); //start line
Declare "ckpt" at the top of the sketch!
The other errors I will have a look at!
Forget my last post, the "switch case" is slightly more complex than I thought!
I have reactivated all the "if" statements, "switch case" was not doing what I wanted it to do, but I have left the single function in this version.
Try compiling this, the errors should be gone, hopefully!!
now im geting this error
chip2_10-27-2019:350:35: error: the value of 'rwaDiv' is not usable in a constant expression
case (cmmoved >= 2512 + rwaDiv):
^