-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.c
142 lines (123 loc) · 3.6 KB
/
main.c
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
/*
* Copyright (c) 2015,
* National Instruments Corporation.
* All rights reserved.
*/
#include <stdio.h>
#include <stdlib.h>
#include "MyRio.h"
#define MEM_WIDTH 10
#define MEM_DEPTH 5
#define MEM_ITER (int)memory[MEM_WIDTH+1]
#define MEM_VAL memory[MEM_WIDTH+2]
void initializeMemory(double**);
void freeTheMemory(double**);
int getIndex(int);
/**
* Overview:
* myRIO main function. This template contains basic code to open the myRIO
* FPGA session. You can remove this code if you only need to use the UART.
*
* Code in myRIO example projects is designed for reuse. You can copy source
* files from the example project to use in this template.
*
* Note:
* The Eclipse project defines the preprocessor symbol for the myRIO-1900.
* Change the preprocessor symbol to use this example with the myRIO-1950.
*/
int main(int argc, char **argv)
{
NiFpga_Status status;
/*
* Open the myRIO NiFpga Session.
* This function MUST be called before all other functions. After this call
* is complete the myRIO target will be ready to be used.
*/
status = MyRio_Open();
if (MyRio_IsNotSuccess(status))
{
return status;
}
/*
* Your application code goes here.
*/
//we're gonna need lots of ram boss
//layout of memory:
//0-MEM_WIDTH: memory blocks of MEM_DEPTH size
//MEM_ITER = MEM_WIDTH+1: a table of current indexes for each block
//MEM_VAL = MEM_WIDTH+2: a table of current averages for each block
double reference[MEM_WIDTH] = [0,1,5,2,7,8,2,6,7,3]; //something to test with
double* memory[MEM_WIDTH+2];
initializeMemory(memory);
int keepGoing = 1;
int index = getIndex(0);
double Kp = 1;
double Ki = 1;
double lastError[MEM_WIDTH] = {0};
while(keepGoing) {
index = getIndex(index);
double toSet = (Kp*lastError[index])+(Ki*MEM_VAL[index])+reference[index];
setOutput(outputHandle, toSet);
//wait for a bit (outputs aren't instantaneous!)
double result = measureFeedback(feedbackHandle); //some sort of unit conversion needs to take place here
lastError[index] = result-reference[index];
updateBlock(memory, index, lastError[index]);
}
/*
* Close the myRIO NiFpga Session.
* This function MUST be called after all other functions.
*/
freeTheMemory(memory);
status = MyRio_Close();
return status;
}
double measureFeedback(int* feedbackHandle) {
double value = 0
//reads feedback from sensors
//read encoder_joint
//read torque_joint
//read IMU <-- I think I want to thread this. Blocking serial reads suck.
return value;
}
void setOutput(int* outputHandle, double toSet) {
//writes the output value to the registers
}
double updateBlock(double** memory, int blockIndex, double value) {
if(blockIndex >= MEM_WIDTH) {
//error alerting code here
return 0;
}
memory[blockIndex][MEM_ITER[blockIndex]++] = value;
int i = 0;
double valueHolder = 0.0;
for(i = 0; i<MEM_DEPTH; i++) {
valueHolder += memory[blockIndex][i];
}
valueHolder = (valueHolder/(double)MEM_DEPTH);
MEM_VAL[blockIndex] = valueHolder;
return valueHolder;
}
void initializeMemory(double** memory) {
int i = 0;
double memIterator[MEM_DEPTH] = {0.0};
double memVal[MEM_WIDTH] = {0.0};
for(i = 0; i < MEM_WIDTH; i++) {
double * newBlock = malloc(MEM_DEPTH * sizeof(double));
int j = 0;
for(j = 0; j<MEM_DEPTH; j++) {
newBlock[j] = 0.0;
}
memory[i] = newBlock;
}
memory[i+1] = memIterator;
memory[i+2] = memVal;
}
void freeTheMemory(double** memory) {
int i = 0;
for(i = 0; i < MEM_WIDTH; i++) {
free(memory[i]);
}
}
int getIndex(int index) {
return ++index % MEM_WIDTH;
}