ftcsigma2016/ReadAutoOpMode.java at master · sidharths00/ftcsigma2016 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.GyroSensor;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.InterruptedIOException;
import java.util.ArrayList;

import android.os.Environment;
import android.util.Log;
import android.widget.TextView;

/**
 * Created by sidharth on 10/26/16.
 */

@TeleOp(name="Reader Tele Op", group="Linear Opmode")


public class ReadAutoOpMode extends ReadMethods{

    FileInputStream is;
    BufferedReader reader;
    final File file = new File(Environment.getExternalStorageDirectory() + File.separator + "Auto/Report Files/file.txt");
    String fullFile;
    int pos;
    ArrayList<String> commands = new ArrayList<>();
    String arrayItem;
    double leftThrottle;
    double rightThrottle;
    double gyro;
    double leftInch;
    double rightInch;
    BlueNearAutoOpSigma2016 test = new BlueNearAutoOpSigma2016();


    @Override
    public void runOpMode() throws InterruptedException {
        myinit();
        readFile(); //Finds file on the phone and puts the whole file in a string called fullFile
        seperateCommand(); //Cuts the fullFile variable into array items in the array called commands. Cutting = separating at the slash
        waitForStart();
        while (opModeIsActive()) {
            for (int i = 0; i < commands.size(); i++){
                /*telemetry.addData("indside for loop", "true");
                telemetry.addData("Arraylist contents", commands.get(i));
                telemetry.update();*/
                arrayItem = commands.get(i); //Gets the array item
                telemetry.addData("arrayItem:", arrayItem);
                telemetry.update();
                sleep(1000);

                //Find and parse the leftthrottle value
                leftThrottle = Double.parseDouble(arrayItem.substring(arrayItem.indexOf("-") + 1, arrayItem.indexOf("rightThrottle")));
                telemetry.addData("leftThrottle:", leftThrottle);

                arrayItem = arrayItem.substring(arrayItem.indexOf("rightThrottle"));

                //Find and parse the rightthrottle value
                rightThrottle = Double.parseDouble(arrayItem.substring(arrayItem.indexOf("-") + 1, arrayItem.indexOf("gyro")));
                telemetry.addData("rightThrottle:", rightThrottle);

                arrayItem = arrayItem.substring(arrayItem.indexOf("gyro"));

                //Find and parse the gyro value
                gyro = Double.parseDouble(arrayItem.substring(arrayItem.indexOf("-") + 1));
                telemetry.addData("gyro:", gyro);
                telemetry.update();
                sleep(5000);

                //Finds distance value for throttles.
                leftInch = leftThrottle/2250;
                rightInch = rightThrottle/2250;

                //Finds if going straight or turning. Currently a PROBLEM -- see above
                if(leftThrottle > 0 && rightThrottle > 0){
                    test.gyroDrive(0.7, leftInch, gyro);
                }
                if(leftThrottle < 0 && rightThrottle < 0){
                    test.gyroDrive(0.7, leftInch, gyro);
                }
                if((leftThrottle > 0 && rightThrottle < 0) || (leftThrottle < 0 && rightThrottle > 0)){
                    test.gyroTurn(0.5, gyro);
                }
            }
            break;
        }
    }
    public void readFile(){
        if (file.exists()) {
            try {
                is = new FileInputStream(file);
                reader = new BufferedReader(new InputStreamReader(is));
                String line = "";
                fullFile = "";
                while (line != null) {
                    line = reader.readLine();
                    fullFile += line;
                    //telemetry.addData("Line:", fullFile);
                    //telemetry.update();
                }
            } catch (FileNotFoundException e) {
                e.printStackTrace();
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
        else {
            telemetry.addData("File","Not Found");
            telemetry.update();
        }
        //sleep(20000);
    }

    public void seperateCommand(){
        while (fullFile.length() != 1){
            pos = fullFile.indexOf("/");

            telemetry.addData("arrayItem:", fullFile.substring(0,pos));
            telemetry.addData("length:", fullFile.length());
            telemetry.update();
            sleep(200);

            commands.add(fullFile.substring(0,pos));
            if(pos < fullFile.length() - 8){
                telemetry.addData("arrayItem:", fullFile.substring(0,pos));
                telemetry.addData("length:", fullFile.length());
                telemetry.addData("pos:", pos);
                telemetry.addData("fullFile.length() - 1:", fullFile.length() - 1);
                telemetry.addData("insideIf:", "true");
                telemetry.update();

                fullFile = fullFile.substring(pos + 1);

            }
            else{
                break;
            }
        }
    }
}