1+ package frc .robot ;
2+
3+ import java .io .File ;
4+ import java .io .FileNotFoundException ;
5+ import java .io .FileReader ;
6+ import java .io .FileWriter ;
7+ import java .io .IOException ;
8+ import java .io .Reader ;
9+ import java .util .ArrayList ;
10+ import java .util .List ;
11+
12+ import org .apache .commons .math3 .stat .regression .OLSMultipleLinearRegression ;
13+ import org .apache .commons .lang3 .ArrayUtils ;
14+ import org .apache .commons .csv .CSVRecord ;
15+ import org .apache .commons .csv .CSVFormat ;
16+ import org .apache .commons .csv .CSVParser ;
17+
18+ public class DrivetrainCharAnalysis {
19+ // Ordinary Least Squares approach (multi-variable approach)
20+ public static void ordinaryLeastSquares (String file1 , String file2 , String outfile ) {
21+ double leftKv , rightKv ;
22+ double leftKa , rightKa ;
23+ double leftVoltageIntercept , rightVoltageIntercept ;
24+ int spread = 30 ;
25+
26+ double [][] returns ;
27+ double [] params , leftVelocities , rightVelocities , voltages , leftAccelerations , rightAccelerations ;
28+
29+ returns = parseCSV (file1 , spread );
30+ leftVelocities = returns [0 ];
31+ rightVelocities = returns [1 ];
32+ voltages = returns [2 ];
33+ leftAccelerations = returns [3 ];
34+ rightAccelerations = returns [4 ];
35+
36+ returns = parseCSV (file2 , spread );
37+ leftVelocities = ArrayUtils .addAll (leftVelocities , returns [0 ]);
38+ rightVelocities = ArrayUtils .addAll (rightVelocities , returns [1 ]);
39+ voltages = ArrayUtils .addAll (voltages , returns [2 ]);
40+ leftAccelerations = ArrayUtils .addAll (leftAccelerations , returns [3 ]);
41+ rightAccelerations = ArrayUtils .addAll (rightAccelerations , returns [4 ]);
42+
43+ double [][] xs = new double [voltages .length ][2 ];
44+ double [] ys = new double [voltages .length ];
45+ for (int i = 0 ; i < voltages .length ; i ++) {
46+ xs [i ][0 ] = leftVelocities [i ];
47+ xs [i ][1 ] = leftAccelerations [i ];
48+ ys [i ] = voltages [i ];
49+ }
50+
51+ OLSMultipleLinearRegression algorithm = new OLSMultipleLinearRegression ();
52+ algorithm .newSampleData (ys , xs );
53+ params = algorithm .estimateRegressionParameters ();
54+ // System.out.println(params.length);
55+ leftKv = params [1 ];
56+ leftKa = params [2 ];
57+ leftVoltageIntercept = params [0 ];
58+
59+ xs = new double [voltages .length ][2 ];
60+ ys = new double [voltages .length ];
61+ for (int i = 0 ; i < voltages .length ; i ++) {
62+ xs [i ][0 ] = rightVelocities [i ];
63+ xs [i ][1 ] = rightAccelerations [i ];
64+ ys [i ] = voltages [i ];
65+ }
66+
67+ algorithm = new OLSMultipleLinearRegression ();
68+ algorithm .newSampleData (ys , xs );
69+ params = algorithm .estimateRegressionParameters ();
70+ // System.out.println(params.length);
71+ rightKv = params [1 ];
72+ rightKa = params [2 ];
73+ rightVoltageIntercept = params [0 ];
74+
75+ FileWriter f ;
76+ try {
77+ f = new FileWriter (new File (outfile ), true );
78+ f .append (leftKv + "," + leftKa + "," + leftVoltageIntercept + "\r \n " );
79+ f .append (rightKv + "," + rightKa + "," + rightVoltageIntercept + "\r \n " );
80+ f .close ();
81+ } catch (IOException e ) {
82+ e .printStackTrace ();
83+ }
84+ }
85+
86+ // OUTDATED
87+ // Simplistic approach to the problem.
88+ public static void simpleRegression (String file1 , String file2 , String outfile ) {
89+ // Argument 1 should be the filepath of the linear increase CSV file.
90+ // Argument 2 should be the filepath of the stepwise increase CSV file.
91+ double kv = 0.0 ; // The parameter for velocity in the drivetrain characterization formula
92+ double ka = 0.0 ; // The parameter for acceleration in the drivetrain characterization formula
93+ double voltageIntercept = 0.0 ;
94+ int spread = 18 ; // How much data should our difference quotient cover?
95+
96+ double [][] returns ;
97+ double [] params , velocities , voltages , accelerations , stepwise_x ;
98+
99+ returns = parseCSV (file1 , spread );
100+ velocities = returns [0 ];
101+ voltages = returns [1 ];
102+ accelerations = returns [2 ];
103+
104+ // System.out.println(linearVelocities);
105+ params = simpleRegressionFormula (voltages , velocities );
106+ kv = 1 / params [1 ]; // Voltseconds / inches
107+ voltageIntercept = -params [0 ] / params [1 ]; // Think of this as -(b/m) in y = mx + b
108+
109+ returns = parseCSV (file2 , spread );
110+ velocities = returns [0 ];
111+ voltages = returns [1 ];
112+ accelerations = returns [2 ];
113+ stepwise_x = velocities ; // To define the size of stepwise_x
114+
115+ for (int i = 0 ; i < velocities .length ; i ++) {
116+ stepwise_x [i ] = voltages [i ] - (kv * velocities [i ] + voltageIntercept );
117+ }
118+
119+ params = simpleRegressionFormula (stepwise_x , accelerations );
120+ ka = 1 / params [1 ]; // Volt * (seconds^2) / inches.
121+
122+ System .out .print ("Velocity Constant is " + Double .toString (12 * kv ) /* Change inches to feet in printout */ );
123+ System .out .print (" and the Acceleration Constant is " + Double .toString (12 * ka ));
124+ System .out .print (" with a voltage intercept of " + Double .toString (voltageIntercept ) + "\n " );
125+
126+ FileWriter f ;
127+ try {
128+ f = new FileWriter (new File (outfile ));
129+ f .append (kv + "," + ka + "," + voltageIntercept );
130+ f .close ();
131+ } catch (IOException e ) {
132+ e .printStackTrace ();
133+ }
134+ }
135+
136+ public static double [] simpleRegressionFormula (double [] xs , double [] ys ) {
137+ double sx , sxx , sy , sxy , a , b ;
138+ sx = sxx = sy = sxy = a = b = 0.0 ;
139+ int n = xs .length ;
140+
141+ for (int i = 0 ; i < n ; i ++) {
142+ sx += xs [i ];
143+ sxx += xs [i ] * xs [i ];
144+ sxy += xs [i ] * ys [i ];
145+ sy += ys [i ];
146+ }
147+
148+ b = (n * sxy - sx * sy ) / (n * sxx - sx * sx );
149+ a = (sy - b * sx ) / n ;
150+
151+ double [] params = { a , b };
152+ return params ;
153+ }
154+
155+ public static double [][] parseCSV (String filename , int spread ) {
156+ try {
157+ Reader in = new FileReader (filename );
158+ CSVParser csvParser = new CSVParser (in , CSVFormat .DEFAULT );
159+
160+ List <CSVRecord > records = csvParser .getRecords ();
161+ int num_values = records .size ();
162+ List <Double > leftVelocities , rightVelocities , voltages , leftAccelerations , rightAccelerations ;
163+ leftVelocities = new ArrayList <Double >();
164+ rightVelocities = new ArrayList <Double >();
165+ voltages = new ArrayList <Double >();
166+ leftAccelerations = new ArrayList <Double >();
167+ rightAccelerations = new ArrayList <Double >();
168+ leftAccelerations .add (0.0 );
169+ rightAccelerations .add (0.0 );
170+
171+ for (int n = 0 ; n < num_values ; n ++) {
172+ CSVRecord record = records .get (n );
173+ if (!record .get (0 ).equals ("Timestamp (s)" )) {
174+ double v1 = Math .abs (Double .valueOf (record .get (2 )));
175+ double v2 = Math .abs (Double .valueOf (record .get (3 )));
176+ double v = Double .valueOf (record .get (1 ));
177+
178+ if (leftVelocities .size () >= spread ) {
179+ double a1 = (v1 - leftVelocities .get (leftVelocities .size () - spread )) / (spread * 0.02 );
180+ double a2 = (v2 - rightVelocities .get (rightVelocities .size () - spread )) / (spread * 0.02 );
181+ leftAccelerations .add (Math .abs (a1 ));
182+ rightAccelerations .add (Math .abs (a2 ));
183+ }
184+
185+ if (leftVelocities .size () < num_values - spread ) {
186+ leftVelocities .add (v1 );
187+ rightVelocities .add (v2 );
188+ voltages .add (v );
189+ }
190+ }
191+ }
192+
193+ Double [][] Returns = { leftVelocities .toArray (new Double [leftVelocities .size ()]),
194+ rightVelocities .toArray (new Double [rightVelocities .size ()]),
195+ voltages .toArray (new Double [voltages .size ()]),
196+ leftAccelerations .toArray (new Double [leftAccelerations .size ()]),
197+ rightAccelerations .toArray (new Double [rightAccelerations .size ()]) };
198+
199+ double [][] returns = new double [5 ][voltages .size ()];
200+ for (int i = 0 ; i < 5 ; i ++) {
201+ for (int j = 0 ; j < voltages .size (); j ++) {
202+ returns [i ][j ] = Returns [i ][j ];
203+ }
204+ }
205+
206+ return returns ;
207+ } catch (FileNotFoundException fnf ) {
208+ fnf .printStackTrace ();
209+ return null ;
210+ } catch (IOException io ) {
211+ io .printStackTrace ();
212+ return null ;
213+ }
214+ }
215+ }
0 commit comments