Advertisement
j0h

tankDrive

j0h
Jun 25th, 2022
1,146
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <CSV_Parser.h>
  2. #include <SPI.h>
  3. #include <SD.h>
  4.  
  5.  
  6. //const int chipSelect = 10;
  7. void Motor1(int d, int t);
  8. void Motor2(int d, int t);
  9. void halt();  //stop everything
  10. void setup() {
  11.  
  12. //motor drive pins
  13. pinMode(20, OUTPUT);
  14. pinMode(21, OUTPUT);
  15.  
  16. pinMode(22, OUTPUT);
  17. pinMode(23, OUTPUT);
  18.  
  19.   Serial.begin(9600);
  20.  
  21.   delay(5000);
  22.  
  23.   Serial.print("Initializing SD card...");
  24.  
  25.   // see if the card is present and can be initialized:
  26.   if (!SD.begin(BUILTIN_SDCARD)) {
  27.     Serial.println("Card failed, or not present");
  28.    
  29.     // don't do anything more:
  30.     while (1);
  31.   }
  32.   Serial.println("card initialized.");
  33.  
  34.  
  35.   CSV_Parser cp(/*format*/ "dd", /*has_header*/ false, /*delimiter*/ ',');
  36.  
  37.   // The line below (readSDfile) wouldn't work if SD.begin wasn't called before.
  38.   // readSDfile can be used as conditional, it returns 'false' if the file does not exist.
  39.   if (cp.readSDfile("/nav0.txt")) {
  40.     int16_t *column_1 = (int16_t*)cp["column_1"];
  41.     int16_t *column_2 = (int16_t*)cp["column_2"];
  42.  
  43.     if (column_1 && column_2) {
  44.       for(int row = 0; row < cp.getRowsCount(); row++) {
  45.         Serial.print("row = ");
  46.         Serial.print(row, DEC);
  47.         Serial.print(", column_1 = ");
  48.         Serial.print(column_1[row], DEC);
  49.         Serial.print(", column_2 = ");
  50.         Serial.println(column_2[row], DEC);
  51.       }
  52.     } else {
  53.       Serial.println("ERROR: At least 1 of the columns was not found, something went wrong.");
  54.     }
  55.    
  56.     // output parsed values (allows to check that the file was parsed correctly)
  57.     cp.print(); // assumes that "Serial.begin()" was called before (otherwise it won't work)
  58.    
  59.   } else {
  60.     Serial.println("ERROR: File called '/nav.txt' does not exist...");
  61.   }
  62. }
  63.  
  64. void loop() {
  65.   //+int fwd -int reverse 0 stop
  66.   //fuuuq if halt() is used, I cant drive 2 motors concurently?
  67. Motor1(1,50);
  68. Motor2(1,50);
  69. //ok use x,y cords as motor timing. get direction from.... diff of current number-previous number (x,y - x1,y1) x is 2 reads before x1
  70.  
  71. //delay(10000);
  72. }
  73.  
  74. void Motor1(int d, int t){ //right
  75. //right rev 20
  76. //right fwd 21
  77. if(d==0){
  78. halt();
  79. }
  80. if(d>=1){
  81.     for(int i=0; i<t; i++){
  82.       digitalWrite(21,HIGH);
  83.       delay(i);
  84.       Serial.println(i);
  85.       }
  86.       halt();  
  87.       }
  88.   if(d<0){
  89.     //reverse
  90.     for(int i=0; i<t; i++){
  91.       digitalWrite(20,HIGH);
  92.       delay(i);
  93.       Serial.println(i);
  94.       }
  95.       halt();    
  96.     }
  97.      halt();
  98.   }
  99.  
  100. void Motor2(int d, int t){ // left
  101.   //run then stop
  102. //left fwd 22
  103. //left rev 23
  104. if(d==0){
  105. halt();
  106. }
  107. if(d>=1){
  108.     for(int i=0; i<t; i++){
  109.       digitalWrite(22,HIGH);
  110.       delay(i);
  111.       }
  112.       halt();  
  113.       }
  114.   if(d<0){
  115.     //reverse
  116.     for(int i=0; i<t; i++){
  117.       digitalWrite(23,HIGH);
  118.       delay(i);
  119.       }
  120.       halt();    
  121.     }
  122.      halt();
  123.     }
  124.  
  125. void halt(){
  126. digitalWrite(20,LOW);
  127. digitalWrite(21,LOW);
  128. digitalWrite(22,LOW);
  129. digitalWrite(23,LOW);
  130. delay(100);  
  131.   }
  132.    
Advertisement
RAW Paste Data Copied
Advertisement