Revision 0:76c761d3caf1, committed 2021-03-20

Comitter:

adimmit

Date:

Sat Mar 20 18:38:51 2021 +0000

Child:

1:aa253b5f5b65

Commit message:

updated to 18 motors

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.gitignore	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,4 @@
+.build
+.mbed
+projectfiles
+*.py*

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTRIBUTING.md	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,5 @@
+# Contributing to Mbed OS
+
+Mbed OS is an open-source, device software platform for the Internet of Things. Contributions are an important part of the platform, and our goal is to make it as simple as possible to become a contributor.
+
+To encourage productive collaboration, as well as robust, consistent and maintainable code, we have a set of guidelines for [contributing to Mbed OS](https://os.mbed.com/docs/mbed-os/latest/contributing/index.html).

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/README.md	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,64 @@
+![](./resources/official_armmbed_example_badge.png)
+# Blinky Mbed OS example
+
+The example project is part of the [Arm Mbed OS Official Examples](https://os.mbed.com/code/) and is the [getting started example for Mbed OS](https://os.mbed.com/docs/mbed-os/v5.14/quick-start/index.html). It contains an application that repeatedly blinks an LED on supported [Mbed boards](https://os.mbed.com/platforms/).
+
+You can build the project with all supported [Mbed OS build tools](https://os.mbed.com/docs/mbed-os/latest/tools/index.html). However, this example project specifically refers to the command-line interface tool [Arm Mbed CLI](https://github.com/ARMmbed/mbed-cli#installing-mbed-cli).
+(Note: To see a rendered example you can import into the Arm Online Compiler, please see our [import quick start](https://os.mbed.com/docs/mbed-os/latest/quick-start/online-with-the-online-compiler.html#importing-the-code).)
+
+1. [Install Mbed CLI](https://os.mbed.com/docs/mbed-os/latest/quick-start/offline-with-mbed-cli.html).
+
+1. Clone this repository on your system, and change the current directory to where the project was cloned:
+
+    ```bash
+    $ git clone git@github.com:armmbed/mbed-os-example-blinky && cd mbed-os-example-blinky
+    ```
+
+    Alternatively, you can download the example project with Arm Mbed CLI using the `import` subcommand:
+
+    ```bash
+    $ mbed import mbed-os-example-blinky && cd mbed-os-example-blinky
+    ```
+
+
+## Application functionality
+
+The `main()` function is the single thread in the application. It toggles the state of a digital output connected to an LED on the board.
+
+## Building and running
+
+1. Connect a USB cable between the USB port on the board and the host computer.
+2. <a name="build_cmd"></a> Run the following command to build the example project and program the microcontroller flash memory:
+    ```bash
+    $ mbed compile -m <TARGET> -t <TOOLCHAIN> --flash
+    ```
+The binary is located at `./BUILD/<TARGET>/<TOOLCHAIN>/mbed-os-example-blinky.bin`.
+
+Alternatively, you can manually copy the binary to the board, which you mount on the host computer over USB.
+
+Depending on the target, you can build the example project with the `GCC_ARM`, `ARM` or `IAR` toolchain. After installing Arm Mbed CLI, run the command below to determine which toolchain supports your target:
+
+```bash
+$ mbed compile -S
+```
+
+## Expected output
+The LED on your target turns on and off every 500 milliseconds.
+
+
+## Troubleshooting
+If you have problems, you can review the [documentation](https://os.mbed.com/docs/latest/tutorials/debugging.html) for suggestions on what could be wrong and how to fix it.
+
+## Related Links
+
+* [Mbed OS Stats API](https://os.mbed.com/docs/latest/apis/mbed-statistics.html).
+* [Mbed OS Configuration](https://os.mbed.com/docs/latest/reference/configuration.html).
+* [Mbed OS Serial Communication](https://os.mbed.com/docs/latest/tutorials/serial-communication.html).
+* [Mbed OS bare metal](https://os.mbed.com/docs/mbed-os/latest/reference/mbed-os-bare-metal.html).
+* [Mbed boards](https://os.mbed.com/platforms/).
+
+### License and contributions
+
+The software is provided under Apache-2.0 license. Contributions to this project are accepted under the same license. Please see contributing.md for more info.
+
+This project contains code from other projects. The original license text is included in those source files. They must comply with our license guide.

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/leg_message.h	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,60 @@
+#ifndef _leg_message
+#define _leg_message
+
+#include <stdint.h>
+
+// 88 bytes
+// 44 16-bit words
+struct spi_data_t
+{
+    float q_1s[3];
+    float q_2s[3];
+    float q_3s[3];
+    float qd_1s[3];
+    float qd_2s[3];
+    float qd_3s[3];
+    int32_t flags[3];
+    int32_t checksum;
+};
+
+// 196 bytes
+// 98 16-bit words
+struct spi_command_t
+{
+    float q_des_1s[3];
+    float q_des_2s[3];
+    float q_des_3s[3];
+    float qd_des_1s[3];
+    float qd_des_2s[3];
+    float qd_des_3s[3];
+    float kp_1s[3];
+    float kp_2s[3];
+    float kp_3s[3];
+    float kd_1s[3];
+    float kd_2s[3];
+    float kd_3s[3];
+    float tau_1s_ff[3];
+    float tau_2s_ff[3];
+    float tau_3s_ff[3];
+    int32_t flags[3];
+    int32_t checksum;
+};
+
+
+
+struct joint_control{
+    float p_des, v_des, kp, kd, t_ff;
+    };
+    
+struct joint_state{
+    float p, v, t;
+    };
+    
+struct grouped_act_state{
+    joint_state a, h, k;
+    };
+struct grouped_act_control{
+    joint_control a, h, k;
+    }
+    ;
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,708 @@
+
+                                                                                    //GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!!
+                                                                                    //ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING
+
+#include "mbed.h"
+#include "math_ops.h"
+#include <cstring>
+#include "leg_message.h"
+
+// length of receive/transmit buffers
+#define RX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
+#define TX_LEN 98                                                                   //CHECK THESE BUFFER LENGHTS
+
+// length of outgoing/incoming messages
+#define DATA_LEN 44                                                                 //CHECK THESE BUFFER LENGHTS
+#define CMD_LEN  98                                                                 //CHECK THESE BUFFER LENGHTS
+
+// Master CAN ID ///
+#define CAN_ID 0x0
+
+
+/// Value Limits ///
+ #define P_MIN -12.5f
+ #define P_MAX 12.5f
+ #define V_MIN -65.0f
+ #define V_MAX 65.0f
+ #define KP_MIN 0.0f
+ #define KP_MAX 500.0f
+ #define KD_MIN 0.0f
+ #define KD_MAX 5.0f
+ #define T_MIN -18.0f
+ #define T_MAX 18.0f
+ 
+ /// Joint Soft Stops ///
+ #define A_LIM_P 1.5f
+ #define A_LIM_N -1.5f
+ #define H_LIM_P 5.0f
+ #define H_LIM_N -5.0f
+ #define K_LIM_P 0.2f
+ #define K_LIM_N 7.7f
+ #define KP_SOFTSTOP 100.0f
+ #define KD_SOFTSTOP 0.4f;
+
+#define ENABLE_CMD 0xFFFF
+#define DISABLE_CMD 0x1F1F
+
+spi_data_t spi_data; // data from spine to up
+spi_command_t spi_command; // data from up to spine
+
+// spi buffers
+uint16_t rx_buff[RX_LEN];
+uint16_t tx_buff[TX_LEN];
+
+DigitalOut led(PC_5);
+
+
+Serial       pc(PA_2, PA_3);
+CAN          can1(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name
+CAN          can2(PA_11, PA_12, 1000000);  // CAN Rx pin name, CAN Tx pin name
+CAN          can3(PA_8, PA_15, 1000000);  // CAN Rx pin name, CAN Tx pin name //CAN4 on board
+
+CANMessage   rxMsg1, rxMsg2, rxMsg3;
+CANMessage   txMsg1, txMsg2, txMsg3;
+CANMessage   q11_can, q21_can, q31_can, q12_can, q22_can, q32_can, q13_can, q23_can, q33_can;    //TX Messages
+int                     ledState;
+Ticker                  sendCAN;
+int                     counter = 0;
+volatile bool           msgAvailable = false;
+Ticker loop;
+
+int spi_enabled = 0;
+InterruptIn cs(PA_4);
+DigitalIn estop(PB_15);
+//SPISlave spi(PA_7, PA_6, PA_5, PA_4);
+
+
+grouped_act_state g1_state, g2_state, g3_state;
+grouped_act_control q1_control, q2_control, q3_control;
+
+uint16_t x = 0;
+uint16_t x2 = 0;
+uint16_t count = 0;
+uint16_t counter2 = 0;                                                              //SEE IF WE NEED TO UPDATE THESE TO ADD COUNTER3 AND X3
+
+int control_mode = 1;
+int is_standing = 0;                                                                //SEE IF WE STILL NEED THE STANDING THING
+int enabled = 0;
+
+// generates fake spi data from spi command
+void test_control();                                                                //MAY NEED TO GET RID OF THIS?
+void control();
+
+
+/// CAN Command Packet Structure ///
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+
+void pack_cmd(CANMessage * msg, joint_control joint){
+     
+     /// limit data to be within bounds ///
+     float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
+     float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
+     float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
+     float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
+     float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
+     /// convert floats to unsigned ints ///
+     uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
+     uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
+     uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
+     uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
+     uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
+     /// pack ints into the can buffer ///
+     msg->data[0] = p_int>>8;                                       
+     msg->data[1] = p_int&0xFF;
+     msg->data[2] = v_int>>4;
+     msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
+     msg->data[4] = kp_int&0xFF;
+     msg->data[5] = kd_int>>4;
+     msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
+     msg->data[7] = t_int&0xff;
+     }
+     
+/// CAN Reply Packet Structure ///
+/// 16 bit position, between -4*pi and 4*pi
+/// 12 bit velocity, between -30 and + 30 rad/s
+/// 12 bit current, between -40 and 40;
+/// CAN Packet is 5 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], current[11-8]]
+/// 4: [current[7-0]]
+
+void unpack_reply(CANMessage msg, grouped_act_state * group){
+    /// unpack ints from can buffer ///
+    uint16_t id = msg.data[0];
+    uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
+    uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
+    uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
+    /// convert uints to floats ///
+    float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
+    float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
+    float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
+    
+    if(id==1){
+        group->a.p = p;
+        group->a.v = v;
+        group->a.t = t;
+        }
+    else if(id==2){
+        group->h.p = p;
+        group->h.v = v;
+        group->h.t = t;
+        }
+    else if(id==3){
+        group->k.p = p;
+        group->k.v = v;
+        group->k.t = t;
+        }
+    } 
+
+ void rxISR1() {
+    can1.read(rxMsg1);                    // read message into Rx message storage
+    unpack_reply(rxMsg1, &g1_state);
+}
+void rxISR2(){
+    can2.read(rxMsg2);
+    unpack_reply(rxMsg2, &g2_state);
+    }
+void rxISR3(){
+    can3.read(rxMsg3);
+    unpack_reply(rxMsg3, &g3_state);
+    }
+    
+void PackAll(){
+    pack_cmd(&q11_can, q1_control.a); 
+    pack_cmd(&q21_can, q1_control.h); 
+    pack_cmd(&q31_can, q1_control.k); 
+    pack_cmd(&q12_can, q2_control.a); 
+    pack_cmd(&k22_can, q2_control.h); 
+    pack_cmd(&k32_can, q2_control.k); 
+    pack_cmd(&q13_can, q3_control.a); 
+    pack_cmd(&k23_can, q3_control.h); 
+    pack_cmd(&k33_can, q3_control.k); 
+    
+    }
+void WriteAll(){
+    //toggle = 1;
+    can1.write(q11_can);
+    wait(.00002);
+    can2.write(q12_can);
+    wait(.00002);
+    can3.write(q13_can);
+    wait(.00002);
+    can1.write(q21_can);
+    wait(.00002);
+    can2.write(q22_can);
+    wait(.00002);
+    can3.write(q23_can);
+    wait(.00002);
+    can1.write(q31_can);
+    wait(.00002);
+    can2.write(q32_can);
+    wait(.00002);
+    can3.write(q33_can);
+    wait(.00002);
+    //toggle = 0;
+    }
+
+void sendCMD(){
+    counter ++;
+
+    PackAll();
+
+    if(counter>100){
+        printf("%.3f %.3f %.3f   %.3f %.3f %.3f   %.3f %.3f %.3f\n\r", q1_state.a.p, q1_state.h.p, q1_state.k.p, q2_state.a.p, q2_state.h.p, q_state.k.p, q3_state.a.p, q3_state.h.p, q3_state.k.p);
+        counter = 0 ;
+        }
+    
+    WriteAll();
+    }
+
+
+
+    
+void Zero(CANMessage * msg){
+    msg->data[0] = 0xFF;
+    msg->data[1] = 0xFF;
+    msg->data[2] = 0xFF;
+    msg->data[3] = 0xFF;
+    msg->data[4] = 0xFF;
+    msg->data[5] = 0xFF;
+    msg->data[6] = 0xFF;
+    msg->data[7] = 0xFE;
+    WriteAll();
+    }
+
+void EnterMotorMode(CANMessage * msg){
+    msg->data[0] = 0xFF;
+    msg->data[1] = 0xFF;
+    msg->data[2] = 0xFF;
+    msg->data[3] = 0xFF;
+    msg->data[4] = 0xFF;
+    msg->data[5] = 0xFF;
+    msg->data[6] = 0xFF;
+    msg->data[7] = 0xFC;
+    //WriteAll();
+    }
+    
+void ExitMotorMode(CANMessage * msg){
+    msg->data[0] = 0xFF;
+    msg->data[1] = 0xFF;
+    msg->data[2] = 0xFF;
+    msg->data[3] = 0xFF;
+    msg->data[4] = 0xFF;
+    msg->data[5] = 0xFF;
+    msg->data[6] = 0xFF;
+    msg->data[7] = 0xFD;
+    //WriteAll();
+    }
+void serial_isr(){
+     /// handle keyboard commands from the serial terminal ///
+     while(pc.readable()){
+        char c = pc.getc();
+        //led = !led;
+        switch(c){
+            case(27):
+                //loop.detach();
+                printf("\n\r exiting motor mode \n\r");
+                ExitMotorMode(&q11_can);
+                ExitMotorMode(&q21_can);
+                ExitMotorMode(&q31_can);
+                ExitMotorMode(&q12_can);
+                ExitMotorMode(&q22_can);
+                ExitMotorMode(&q32_can);
+                ExitMotorMode(&q13_can);
+                ExitMotorMode(&q23_can);
+                ExitMotorMode(&q33_can);
+                enabled = 0;
+                break;
+            case('m'):
+                printf("\n\r entering motor mode \n\r");
+                EnterMotorMode(&q11_can);
+                EnterMotorMode(&q21_can);
+                EnterMotorMode(&q31_can);
+                EnterMotorMode(&q12_can);
+                EnterMotorMode(&q22_can);
+                EnterMotorMode(&q32_can);
+                EnterMotorMode(&q13_can);
+                EnterMotorMode(&q23_can);
+                EnterMotorMode(&q33_can);
+                wait(.5);
+                enabled = 1;
+                //loop.attach(&sendCMD, .001);
+                break;
+            case('s'):
+                printf("\n\r standing \n\r");
+                counter2 = 0;
+                is_standing = 1;
+                //stand();
+                break;
+            case('z'):
+                printf("\n\r zeroing \n\r");
+                Zero(&q11_can);
+                Zero(&q21_can);
+                Zero(&q31_can);
+                Zero(&q12_can);
+                Zero(&q22_can);
+                Zero(&q32_can);
+                Zero(&q13_can);
+                Zero(&q23_can);
+                Zero(&q33_can);
+                break;
+            }
+        }
+        WriteAll();
+        
+    }
+    
+uint32_t xor_checksum(uint32_t* data, size_t len)
+{
+    uint32_t t = 0;
+    for(int i = 0; i < len; i++)   
+        t = t ^ data[i];
+    return t;
+}
+
+void spi_isr(void)
+{
+    GPIOC->ODR |= (1 << 8);
+    GPIOC->ODR &= ~(1 << 8);
+    int bytecount = 0;
+    SPI1->DR = tx_buff[0];
+    while(cs == 0) {
+        if(SPI1->SR&0x1) {
+            [bytecount] = SPI1->DR;
+            bytecount++;
+            if(bytecount<TX_LEN) {
+                SPI1->DR = tx_buff[bytecount];
+            }
+        }
+
+    }
+    
+    // after reading, save into spi_command
+    // should probably check checksum first!
+    uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
+    for(int i = 0; i < CMD_LEN; i++)
+    {
+        ((uint16_t*)(&spi_command))[i] = rx_buff[i];
+    }
+    
+    // run control, which fills in tx_buff for the next iteration
+    if(calc_checksum != spi_command.checksum){
+        spi_data.flags[1] = 0xdead;}
+        
+    //test_control();
+    //spi_data.q_abad[0] = 12.0f;
+    control();
+    PackAll();
+    WriteAll();
+
+
+    //for (int i = 0; i<TX_LEN; i++) {
+     //   tx_buff[i] = 2*rx_buff[i];
+    //}
+//    for (int i=0; i<TX_LEN; i++) {
+//        //printf("%d ", rx_buff[i]);
+//    }
+    //printf("\n\r");
+}
+
+int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
+    if((state.p)>=limit_p){
+        //control->p_des = limit_p;
+        control->v_des = 0.0f;
+        control->kp = 0;
+        control->kd = KD_SOFTSTOP;
+        control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
+        return 1;
+    }
+    else if((state.p)<=limit_n){
+        //control->p_des = limit_n;
+        control->v_des = 0.0f;
+        control->kp = 0;
+        control->kd = KD_SOFTSTOP;
+        control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
+        return 1;
+    }
+    return 0;
+    
+    }
+    
+    
+void control()
+{
+    
+    if(((spi_command.flags[0]&0x1)==1)  && (enabled==0)){
+        enabled = 1;
+        //BUS ONE
+        EnterMotorMode(&q11_can);
+        can1.write(q11_can);
+        EnterMotorMode(&q21_can);
+        can1.write(q21_can);
+        EnterMotorMode(&q31_can);
+        can1.write(q31_can);
+        //BUS TWO
+        EnterMotorMode(&q12_can);
+        can2.write(q12_can);
+        EnterMotorMode(&q22_can);
+        can2.write(q22_can);
+        EnterMotorMode(&q31_can);
+        can2.write(q32_can);
+        //BUS THREE
+        EnterMotorMode(&q13_can);
+        can3.write(q13_can);
+        EnterMotorMode(&q23_can);
+        can3.write(q23_can);
+        EnterMotorMode(&q33_can);
+        can3.write(q33_can);
+        printf("e\n\r");
+        return;
+    }
+    else if((((spi_command.flags[0]&0x1))==0)  && (enabled==1)){
+        enabled = 0;
+        //BUS ONE
+        ExitMotorMode(&q11_can);
+        can1.write(q11_can);
+        ExitMotorMode(&q21_can);
+        can1.write(q21_can);
+        ExitMotorMode(&q31_can);
+        can1.write(q31_can);
+        //BUS TWO
+        ExitMotorMode(&q12_can);
+        can2.write(q12_can);
+        ExitMotorMode(&q22_can);
+        can2.write(q22_can);
+        ExitMotorMode(&q31_can);
+        can2.write(q32_can);
+        //BUS THREE
+        ExitMotorMode(&q13_can);
+        can3.write(q13_can);
+        ExitMotorMode(&q23_can);
+        can3.write(q23_can);
+        ExitMotorMode(&q33_can);
+        can3.write(q33_can);
+        printf("x\n\r");
+        return;
+        }
+    
+    spi_data.q_1s[0] = g1_state.a.p;
+    spi_data.q_2s[0] = g1_state.h.p;
+    spi_data.q_3s[0] = g1_state.k.p;
+    spi_data.qd_1s[0] = g1_state.a.v;
+    spi_data.qd_2s[0] = g1_state.h.v;
+    spi_data.qd_3s[0] = g1_state.k.v;
+    
+    spi_data.q_1s[1] = g1_state.a.p;
+    spi_data.q_2s[1] = g1_state.h.p;
+    spi_data.q_3s[1] = g1_state.k.p;
+    spi_data.qd_1s[1] = g1_state.a.v;
+    spi_data.qd_2s[1] = g1_state.h.v;
+    spi_data.qd_3s[1] = g1_state.k.v;
+    
+    spi_data.q_1s[2] = g1_state.a.p;
+    spi_data.q_2s[2] = g1_state.h.p;
+    spi_data.q_3s[2] = g1_state.k.p;
+    spi_data.qd_1s[2] = g1_state.a.v;
+    spi_data.qd_2s[2] = g1_state.h.v;
+    spi_data.qd_3s[2] = g1_state.k.v;
+    
+    
+    
+    if(estop==0){
+        //printf("estopped!!!!\n\r");
+        memset(&q1_control, 0, sizeof(q1_control));
+        memset(&q2_control, 0, sizeof(q2_control));
+        memset(&q3_control, 0, sizeof(q3_control));
+        spi_data.flags[0] = 0xdead;
+        spi_data.flags[1] = 0xdead;
+        spi_data.flags[2] = 0xdead;
+        led = 1;
+        }
+    
+    else{
+        led = 0;
+        
+        memset(&q1_control, 0, sizeof(q1_control));
+        memset(&q2_control, 0, sizeof(q2_control));
+        memset(&q3_control, 0, sizeof(q3_control));
+        
+        
+        q1_control.a.p_des = spi_command.q_des_1s[0];
+        q1_control.a.v_des  = spi_command.qd_des_1s[0];
+        q1_control.a.kp = spi_command.kp_1s[0];
+        q1_control.a.kd = spi_command.kd_1s[0];
+        q1_control.a.t_ff = spi_command.tau_1s_ff[0];
+        
+        q1_control.h.p_des = spi_command.q_des_2s[0];
+        q1_control.h.v_des  = spi_command.qd_des_2s[0];
+        q1_control.h.kp = spi_command.kp_2s[0];
+        q1_control.h.kd = spi_command.kd_2s[0];
+        q1_control.h.t_ff = spi_command.tau_2s_ff[0];
+        
+        q1_control.k.p_des = spi_command.q_des_3s[0];
+        q1_control.k.v_des  = spi_command.qd_des_3s[0];
+        q1_control.k.kp = spi_command.kp_3s[0];
+        q1_control.k.kd = spi_command.kd_3s[0];
+        q1_control.k.t_ff = spi_command.tau_3s_ff[0];
+        
+        q2_control.a.p_des = spi_command.q_des_1s[1];
+        q2_control.a.v_des  = spi_command.qd_des_1s[1];
+        q2_control.a.kp = spi_command.kp_1s[1];
+        q2_control.a.kd = spi_command.kd_1s[1];
+        q2_control.a.t_ff = spi_command.tau_1s_ff[1];
+        
+        q2_control.h.p_des = spi_command.q_des_2s[1];
+        q2_control.h.v_des  = spi_command.qd_des_2s[1];
+        q2_control.h.kp = spi_command.kp_2s[1];
+        q2_control.h.kd = spi_command.kd_2s[1];
+        q2_control.h.t_ff = spi_command.tau_2s_ff[1];
+        
+        q2_control.k.p_des = spi_command.q_des_3s[1];
+        q2_control.k.v_des  = spi_command.qd_des_3s[1];
+        q2_control.k.kp = spi_command.kp_3s[1];
+        q2_control.k.kd = spi_command.kd_3s[1];
+        q2_control.k.t_ff = spi_command.tau_3s_ff[1];
+        
+        q3_control.a.p_des = spi_command.q_des_1s[2];
+        q3_control.a.v_des  = spi_command.qd_des_1s[2];
+        q3_control.a.kp = spi_command.kp_1s[2];
+        q3_control.a.kd = spi_command.kd_1s[2];
+        q3_control.a.t_ff = spi_command.tau_1s_ff[2];
+        
+        q3_control.h.p_des = spi_command.q_des_2s[2];
+        q3_control.h.v_des  = spi_command.qd_des_2s[2];
+        q3_control.h.kp = spi_command.kp_2s[2];
+        q3_control.h.kd = spi_command.kd_2s[2];
+        q3_control.h.t_ff = spi_command.tau_2s_ff[2];
+        
+        q3_control.k.p_des = spi_command.q_des_3s[2];
+        q3_control.k.v_des  = spi_command.qd_des_3s[2];
+        q3_control.k.kp = spi_command.kp_3s[2];
+        q3_control.k.kd = spi_command.kd_3s[2];
+        q3_control.k.t_ff = spi_command.tau_3s_ff[2];
+        
+        
+        spi_data.flags[0] = 0;
+        spi_data.flags[1] = 0;
+        spi_data.flags[2] = 0;
+        spi_data.flags[0] |= softstop_joint(g1_state.a, &q1_control.a, A_LIM_P, A_LIM_N);
+        spi_data.flags[0] |= (softstop_joint(g1_state.h, &q1_control.h, H_LIM_P, H_LIM_N))<<1;
+        //spi_data.flags[0] |= (softstop_joint(g1_state.k, &q1_control.k, K_LIM_P, K_LIM_N))<<2;
+        spi_data.flags[1] |= softstop_joint(g2_state.a, &q2_control.a, A_LIM_P, A_LIM_N);
+        spi_data.flags[1] |= (softstop_joint(g2_state.h, &q2_control.h, H_LIM_P, H_LIM_N))<<1;
+        //spi_data.flags[1] |= (softstop_joint(g2_state.k, &q2_control.k, K_LIM_P, K_LIM_N))<<2;
+        spi_data.flags[2] |= softstop_joint(g3_state.a, &q3_control.a, A_LIM_P, A_LIM_N);
+        spi_data.flags[2] |= (softstop_joint(g3_state.h, &q3_control.h, H_LIM_P, H_LIM_N))<<1;
+        //spi_data.flags[2] |= (softstop_joint(g3_state.k, &q3_control.k, K_LIM_P, K_LIM_N))<<2;
+        
+        //spi_data.flags[0] = 0xbeef;
+        //spi_data.flags[1] = 0xbeef;
+        //PackAll();
+        //WriteAll();
+    }
+    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
+    for(int i = 0; i < DATA_LEN; i++){
+        tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
+    
+}
+    
+
+void test_control()
+{
+    for(int i = 0; i < 3; i++)
+    {
+        spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f;
+        spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f;
+        spi_data.q_3s[i]  = spi_command.q_des_3s[i]  + 1.f;
+        
+        spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f;
+        spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f;
+        spi_data.qd_3s[i]  = spi_command.qd_des_3s[i]  + 1.f;
+    }
+    
+    spi_data.flags[0] = 0xdead;
+    //spi_data.flags[1] = 0xbeef;
+    
+    // only do first 56 bytes of message.
+    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
+    
+    for(int i = 0; i < DATA_LEN; i++)
+        tx_buff[i] = ((uint16_t*)(&spi_data))[i];
+}
+
+void init_spi(void){
+    SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
+    spi->format(16, 0);
+    spi->frequency(12000000);
+    spi->reply(0x0);
+    cs.fall(&spi_isr);
+    printf("done\n\r");
+}
+
+    
+int main() {
+    wait(1);
+    //led = 1;
+    pc.baud(921600);
+    pc.attach(&serial_isr);
+    estop.mode(PullUp);
+    //spi.format(16, 0);
+    //spi.frequency(1000000);
+    //spi.reply(0x0);
+    //cs.fall(&spi_isr);
+
+    //can1.frequency(1000000);                     // set bit rate to 1Mbps
+    //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
+    can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+    //can2.frequency(1000000);                     // set bit rate to 1Mbps
+    //can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
+    can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+    can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
+
+    memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
+    memset(&spi_data, 0, sizeof(spi_data_t));
+    memset(&spi_command,0,sizeof(spi_command_t));
+    
+
+    NVIC_SetPriority(TIM5_IRQn, 1);
+    //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
+    //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
+    
+    pc.printf("\n\r SPIne\n\r");
+    //printf("%d\n\r", RX_ID << 18);
+    
+    q11_can.len = 8;                         //transmit 8 bytes
+    q21_can.len = 8;                         //transmit 8 bytes
+    q31_can.len = 8;
+    q12_can.len = 8;                         //transmit 8 bytes
+    q22_can.len = 8;                         //transmit 8 bytes
+    q32_can.len = 8;
+    q13_can.len = 8;                         //transmit 8 bytes
+    q23_can.len = 8;                         //transmit 8 bytes
+    q33_can.len = 8;
+    rxMsg1.len = 6;                          //receive 6 bytes
+    rxMsg2.len = 6;
+    rxMsg3.len = 6;
+
+    q11_can.id = 0x1;
+    q21_can.id = 0x2;
+    q31_can.id = 0x3;
+    q12_can.id = 0x1;
+    q22_can.id = 0x2;
+    q32_can.id = 0x3;
+    q13_can.id = 0x1;
+    q23_can.id = 0x2;
+    q33_can.id = 0x3;    
+
+    pack_cmd(&q11_can, q1_control.a); 
+    pack_cmd(&q12_can, q2_control.a); 
+    pack_cmd(&q13_can, q3_control.a); 
+    pack_cmd(&q21_can, q1_control.h); 
+    pack_cmd(&q22_can, q2_control.h);
+    pack_cmd(&q23_can, q3_control.h); 
+    pack_cmd(&q31_can, q1_control.k); 
+    pack_cmd(&q32_can, q2_control.k);
+    pack_cmd(&q33_can, q3_control.k); 
+    WriteAll();
+
+
+    // SPI doesn't work if enabled while the CS pin is pulled low
+    // Wait for CS to not be low, then enable SPI
+    if(!spi_enabled){ 
+        while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
+        init_spi();
+        spi_enabled = 1;
+        }
+        
+    //spi_command=set the thing here... 
+            
+    while(1) {
+        pc.printf("test, of SPINE\r\n");
+        counter++;
+        can2.read(rxMsg2);
+        unpack_reply(rxMsg2, &g2_state);
+        can1.read(rxMsg1);                    // read message into Rx message storage
+        unpack_reply(rxMsg1, &g1_state);
+        can3.read(rxMsg3);                    // read message into Rx message storage
+        unpack_reply(rxMsg3, &g3_state);
+        wait_us(10);
+
+        }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops.cpp	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,48 @@
+
+#include "math_ops.h"
+
+
+float fmaxf(float x, float y){
+    /// Returns maximum of x, y ///
+    return (((x)>(y))?(x):(y));
+    }
+
+float fminf(float x, float y){
+    /// Returns minimum of x, y ///
+    return (((x)<(y))?(x):(y));
+    }
+
+float fmaxf3(float x, float y, float z){
+    /// Returns maximum of x, y, z ///
+    return (x > y ? (x > z ? x : z) : (y > z ? y : z));
+    }
+
+float fminf3(float x, float y, float z){
+    /// Returns minimum of x, y, z ///
+    return (x < y ? (x < z ? x : z) : (y < z ? y : z));
+    }
+    
+void limit_norm(float *x, float *y, float limit){
+    /// Scales the lenght of vector (x, y) to be <= limit ///
+    float norm = sqrt(*x * *x + *y * *y);
+    if(norm > limit){
+        *x = *x * limit/norm;
+        *y = *y * limit/norm;
+        }
+    }
+
+
+int float_to_uint(float x, float x_min, float x_max, int bits){
+    /// Converts a float to an unsigned int, given range and number of bits ///
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (int) ((x-offset)*((float)((1<<bits)-1))/span);
+    }
+    
+    
+float uint_to_float(int x_int, float x_min, float x_max, int bits){
+    /// converts unsigned int to float, given range and number of bits ///
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops.h	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,16 @@
+#ifndef MATH_OPS_H
+#define MATH_OPS_H
+
+#define PI 3.14159265359f
+
+#include "math.h"
+
+float fmaxf(float x, float y);
+float fminf(float x, float y);
+float fmaxf3(float x, float y, float z);
+float fminf3(float x, float y, float z);
+void limit_norm(float *x, float *y, float limit);
+int float_to_uint(float x, float x_min, float x_max, int bits);
+float uint_to_float(int x_int, float x_min, float x_max, int bits);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Sat Mar 20 18:38:51 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/adimmit/code/mbed-dev/#993b4d6ff61e

Binary file resources/official_armmbed_example_badge.png has changed