//* Include the SPI library for the arduino boards */

#include <SPI.h>
/* Serial rates for UART */
#define BAUDRATE 115200
/* SPI commands */
#define AMT22_NOP 0x00
#define AMT22_RESET 0x60
#define AMT22_ZERO 0x70
/* Define special ascii characters */
#define NEWLINE 0x0A
#define TAB 0x09
/* We will use these define macros so we can write code once
compatible with 12 or 14 bit encoders */
#define RES12 12
#define RES14 14
/* SPI pins */
#define ENC_0 2
#define ENC_1 3
#define SPI_MOSI 51
#define SPI_MISO 50
#define SPI_SCLK 52

#define motorPin1 12
#define motorPin2 13
int alpha_raw;
int theta_raw;
float alpha_rad;
float theta_rad;
float theta_prev = 0;
float alpha_prev = 0;
float theta_dot;
float alpha_dot;
int t_old = 0;
int t_new = 0;
int dt = 0;
//Q = diag(10,1,5,1) and R=10
//Working Test 1
//float K_theta = -3.1623;
//float K_alpha = 93.4756;
//float K_theta_dot = -4.1504;
//float K_alpha_dot = 12.3874;
//Q = diag(10,1,5,1) and R=10
//Working Test 2
float K_theta = -3.1623;//-3.1623,-2.5623
float K_alpha = 92.84756;//92.84756
float K_theta_dot = -4.0991504;//-3.8991504
float K_alpha_dot = 12.003874;//12.003874

//Q = diag(10,1,5,1) and R=10

//float K_theta = -3.1623;
//float K_alpha = 93.4756;
//float K_theta_dot = -3.9999954;
//float K_alpha_dot = 12.300874;

//Q = diag(10,100, 1, 2) and R=1

//float K_theta = -3.1623;
//float K_alpha = 57.8284;
//float K_theta_dot = -1.9904;
//float K_alpha_dot = 7.5330;
float control_signal;
int motor_voltage = 0;
float scaling_factor = 2.8;
void setup()
{
//Set the modes for the SPI IO
pinMode(SPI_SCLK, OUTPUT);
pinMode(SPI_MOSI, OUTPUT);
pinMode(SPI_MISO, INPUT);
pinMode(ENC_0, OUTPUT);
pinMode(ENC_1, OUTPUT);

//Initialize the UART serial connection for debugging

Serial.begin(BAUDRATE);
//Get the CS line high which is the default inactive state
digitalWrite(ENC_0, HIGH);
digitalWrite(ENC_1, HIGH);
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500 kHz
//start SPI bus
SPI.begin();
}
void loop()
{
delay(5);
// Read alpha (pendulum 1 angle) using SPI
alpha_raw = getPositionSPI(ENC_0, RES14);
alpha_rad = convertToRadians(alpha_raw);
// Read theta (pendulum 2 angle) using SPI
theta_raw = getPositionSPI(ENC_1, RES14);
theta_rad = convertToRadians(theta_raw);
t_new = millis();
dt = t_new - t_old;
 // Calculate angular velocities (x. = dx/dt)
theta_dot = (theta_rad - theta_prev)*1000 / dt;
alpha_dot = (alpha_rad - alpha_prev)*1000 / dt;
// Calculate control signal using LQR (u = -kx)
control_signal = -(K_theta * theta_rad + K_alpha * (alpha_rad-177)
+ K_theta_dot * theta_dot + K_alpha_dot * alpha_dot);
//
// // Scale the control signal to a motor voltage range
// motor_voltage = (int)(control_signal * scaling_factor);
// if (motor_voltage > 0) {
// analogWrite(9, motor_voltage);
// analogWrite(8, 0);
// } else {
// analogWrite(9, 0);
// analogWrite(8, -motor_voltage);
// }
float duty_cycle = control_signal / 905.0;
float c = 1;
if (duty_cycle >=c) {
duty_cycle = c;

} else if (duty_cycle <= -c) {

duty_cycle = -c;
}
if (duty_cycle > 0) {
analogWrite(motorPin2, duty_cycle * 255);
analogWrite(motorPin1, 0);
} else {
analogWrite(motorPin1, -duty_cycle * 255);
analogWrite(motorPin2, 0);
}
// Save current angles for the next iteration
theta_prev = theta_rad;
alpha_prev = alpha_rad;
t_old = t_new;
Serial.print("Theta: ");
Serial.print(theta_rad, DEC);
Serial.write(NEWLINE);
Serial.print(" Alpha: ");
Serial.print(alpha_rad, DEC);
Serial.write(NEWLINE);
Serial.print(" Control Signal: ");
Serial.println( duty_cycle * 255);
//Serial.println( control_signal);
}
float convertToRadians(int raw_value) {
float angle_deg = (raw_value) * (360.0 / 16384);
 return angle_deg;
}
uint16_t getPositionSPI(uint8_t encoder, uint8_t resolution)
{
uint16_t currentPosition; //16-bit response from encoder
bool binaryArray[16]; //after receiving the position we
will populate this array and use it for calculating the checksum
//get first byte which is the high byte, shift it 8 bits. don't
release line for the first byte
currentPosition = spiWriteRead(AMT22_NOP, encoder, false) << 8;
//this is the time required between bytes as specified in the
datasheet.
//We will implement that time delay here, however the arduino is
not the fastest device so the delay
//is likely inherantly there already
delayMicroseconds(3);
//OR the low byte with the currentPosition variable. release line
after second byte
currentPosition |= spiWriteRead(AMT22_NOP, encoder, true);
//run through the 16 bits of position and put each bit into a slot
in the array so we can do the checksum calculation
for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) &
(currentPosition >> (i));
//using the equation on the datasheet we can calculate the
checksums and then make sure they match what the encoder sent
if ((binaryArray[15] == !(binaryArray[13] ^ binaryArray[11] ^
binaryArray[9] ^ binaryArray[7] ^ binaryArray[5] ^ binaryArray[3] ^
binaryArray[1]))
&& (binaryArray[14] == !(binaryArray[12] ^ binaryArray[10]
^ binaryArray[8] ^ binaryArray[6] ^ binaryArray[4] ^ binaryArray[2]
^ binaryArray[0])))
{
//we got back a good position, so just mask away the checkbits
currentPosition &= 0x3FFF;
}
else
{
currentPosition = 0xFFFF; //bad position
}
//If the resolution is 12-bits, and wasn't 0xFFFF, then shift
position, otherwise do nothing
if ((resolution == RES12) && (currentPosition != 0xFFFF))
currentPosition = currentPosition >> 2;
return currentPosition;
}
uint8_t spiWriteRead(uint8_t sendByte, uint8_t encoder, uint8_t
releaseLine)
{
//holder for the received over SPI
uint8_t data;
//set cs low, cs may already be low but there's no issue calling
it again except for extra time
setCSLine(encoder ,LOW);
//There is a minimum time requirement after CS goes low before
data can be clocked out of the encoder.
//We will implement that time delay here, however the arduino is
not the fastest device so the delay
//is likely inherantly there already
delayMicroseconds(3);
//send the command
data = SPI.transfer(sendByte);
delayMicroseconds(3); //There is also a minimum time after
clocking that CS should remain asserted before we release it
setCSLine(encoder, releaseLine); //if releaseLine is high set it
high else it stays low

return data;
}
/*
* This function sets the state of the SPI line. It isn't necessary
but makes the code more readable than having digitalWrite everywhere
* This function takes the pin number of the desired device as an
input
*/
void setCSLine (uint8_t encoder, uint8_t csLine)
{
digitalWrite(encoder, csLine);
}
void setZeroSPI(uint8_t encoder)
{
spiWriteRead(AMT22_NOP, encoder, false);
//this is the time required between bytes as specified in the
datasheet.
//We will implement that time delay here, however the arduino is
not the fastest device so the delay
//is likely inherantly there already
delayMicroseconds(3);

spiWriteRead(AMT22_ZERO, encoder, true);

delay(250); //250 second delay to allow the encoder to reset
}
void resetAMT22(uint8_t encoder)
{
spiWriteRead(AMT22_NOP, encoder, false);
//this is the time required between bytes as specified in the
datasheet.
//We will implement that time delay here, however the arduino is
not the fastest device so the delay
//is likely inherantly there already
delayMicroseconds(3);

spiWriteRead(AMT22_RESET, encoder, true);

delay(250); //250 second delay to allow the encoder to start back

up
}