main.cpp

00001 
00002 //counter for misc purposes
00003 int counter3 = 0;
00004                                                                                     //GO THROUGH AND RE-CHECK ALL THE VARIABLES, STRUCT NAMES, SIZES, BUFFERS + ETC!!!
00005                                                                                     //ALSO GO THROUGH THE COMMENTS TO SEE IF THEY NEED CHANGING
00006 
00007 #include "mbed.h"
00008 #include "math_ops.h"
00009 #include <cstring>
00010 #include "leg_message.h"
00011 
00012 // length of receive/transmit buffers
00013 #define RX_LEN 12                                                                   //CHECK THESE BUFFER LENGHTS
00014 #define TX_LEN 12 
00015 
00016 // length of outgoing/incoming messages
00017 #define CMD_LEN 12 
00018 #define DATA_LEN 12                                                                 //CHECK THESE BUFFER LENGHTS
00019 
00020 // Master CAN ID ///
00021 #define CAN_ID 0x0
00022 
00023 
00024 /// Value Limits ///
00025 #define MIN_CUR -400.0f
00026 #define MAX_CUR 400.0f
00027 
00028 #define MIN_TAU -5.0f
00029 #define MAX_TAU 5.0f
00030 
00031 spi_data_t spi_data; // data from spine to up
00032 
00033 // spi buffers
00034 uint16_t rx_buff[RX_LEN];
00035 uint16_t tx_buff[TX_LEN];
00036 
00037 DigitalOut led(PC_5);
00038 
00039 
00040 Serial       pc(PA_2, PA_3);
00041 CAN          can1(PA_11, PA_12, 1000000);
00042 CAN          can2(PA_8, PA_15, 1000000);
00043 //CAN          can3(PB_12, PB_13, 1000000); //corresponds to bus 1-3-6 (controller 1) or 2-4-5 (controller 2) IN THAT ORDER
00044 
00045 CANMessage   rxMsg1, rxMsg2;
00046 int                     ledState;
00047 Ticker                  sendCAN;
00048 int                     counter = 0;
00049 volatile bool           msgAvailable = false;
00050 Ticker loop;
00051 
00052 int spi_enabled = 0;
00053 InterruptIn cs(PA_4);
00054 DigitalIn estop(PB_15);
00055 
00056 void unpack_reply_4bit(CANMessage msg, spi_data_t * spi_data){
00057     //rx2msg is the data from the torque board (CAN2)
00058     //rx1msg is the data from the upboard (CAN1)
00059         
00060     /// unpack ints from can buffer ///
00061     uint16_t id = msg.id;
00062     uint16_t u1 = (msg.data[0]<<8)|msg.data[1];
00063     uint16_t u2 = (msg.data[2]<<8)|msg.data[3];
00064     
00065     /// convert uints to floats ///
00066     float f1 = uint_to_float(u1, MIN_CUR, MAX_CUR, 16);
00067     float f2 = uint_to_float(u2, MIN_CUR, MAX_CUR, 16);
00068     
00069     if(id==1){
00070         spi_data->hv_current = f1;
00071         spi_data->ext_current = f2;
00072         //pc.printf("ext_current: %f\n", f2);
00073         }
00074     if(id==2){
00075         spi_data->vol = f1;
00076         spi_data->lv_current = f2;
00077         }
00078     } 
00079     
00080 void unpack_reply_2bit(CANMessage msg, spi_data_t * spi_data){
00081     //rx2msg is the data from the torque board (CAN2)
00082     //rx1msg is the data from the upboard (CAN1)
00083     
00084     /// unpack ints from can buffer ///
00085     //uint16_t id = msg.id;
00086     uint16_t tau = (msg.data[0]<<8)|msg.data[1];
00087     /// convert uints to floats ///
00088     float t = uint_to_float(tau, MIN_TAU, MAX_TAU, 16);
00089     //pc.printf("torque: %f\n", t);
00090     spi_data->tau = t;
00091 }
00092 
00093  void rxISR1() {
00094     can1.read(rxMsg1);                    // read message into Rx message storage
00095     unpack_reply_4bit(rxMsg1, &spi_data);
00096 }
00097 void rxISR2(){
00098     can2.read(rxMsg2);
00099     unpack_reply_2bit(rxMsg2, &spi_data);
00100     }
00101     
00102 uint32_t xor_checksum(uint32_t* data, size_t len)
00103 {
00104     uint32_t t = 0;
00105     for(int i = 0; i < len; i++)   
00106         t = t ^ data[i];
00107     return t;
00108 }
00109 
00110 void update_buffers(void) {
00111     //spi_data.checksum = 100;        
00112     spi_data.checksum = xor_checksum((uint32_t*)&spi_data,4); //only do first 16 bytes
00113     for(int i = 0; i < DATA_LEN; i++){
00114         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
00115 }
00116 
00117 void spi_isr(void)
00118 {
00119     //pc.printf("CS ACTIVE\n");
00120     GPIOC->ODR |= (1 << 8);
00121     GPIOC->ODR &= ~(1 << 8);
00122     int bytecount = 0;
00123     SPI1->DR = tx_buff[0];
00124     while(cs == 0) {
00125         if(SPI1->SR&0x1) {
00126             rx_buff[bytecount] = SPI1->DR;
00127             bytecount++;
00128             if(bytecount<TX_LEN) {
00129                 SPI1->DR = tx_buff[bytecount];
00130             }
00131         }
00132     }
00133     
00134     update_buffers();
00135 }
00136 
00137 void init_spi(void){
00138     SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
00139     spi->format(16, 0);
00140     spi->frequency(12000000);
00141     spi->reply(0x0);
00142     cs.fall(&spi_isr);
00143     pc.printf("done\n\r");
00144 }
00145     
00146 int main() {
00147     wait(1);
00148     //led = 1;
00149     pc.baud(115200);                                                                //MAYBE CHANGE THIS IF NEEDED
00150     estop.mode(PullUp);
00151     //spi.format(16, 0);
00152     //spi.frequency(1000000);
00153     //spi.reply(0x0);
00154     //cs.fall(&spi_isr);
00155 
00156     //can1.frequency(1000000);                     // set bit rate to 1Mbps
00157     //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
00158     can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00159     //can2.frequency(1000000);                     // set bit rate to 1Mbps
00160     //can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
00161     can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00162 
00163     memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
00164     memset(&rx_buff, 0, RX_LEN * sizeof(uint16_t));
00165     memset(&spi_data, 0, sizeof(spi_data_t));
00166     
00167 
00168     NVIC_SetPriority(TIM5_IRQn, 1);
00169     
00170     pc.printf("\n\r SPIne SENSOR\n\r");
00171     //printf("%d\n\r", RX_ID << 18);
00172     
00173     //RECIEVE
00174     rxMsg1.len = 4;                          //receive 4 bytes
00175     rxMsg2.len = 2;
00176     
00177     //just debugging things
00178     pc.printf("SETUP VARS ALL DONE\n");
00179 
00180     // SPI doesn't work if enabled while the CS pin is pulled low
00181     // Wait for CS to not be low, then enable SPI
00182 
00183     if(!spi_enabled){ 
00184         while((spi_enabled==0) && (cs.read() ==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
00185         init_spi();
00186         spi_enabled = 1;
00187         pc.printf("SPI ENABLED AND READY\n");
00188         }
00189         
00190     //spi_command=set the thing here... 
00191             
00192     while(1) {
00193         //pc.printf("test, of SPINE\r\n");
00194         //wait(0.0001); //MAKE SURE IT's 1-3k rate, higher we will fill the CAN buffer
00195         
00196         counter++;
00197         can2.read(rxMsg2);
00198         unpack_reply_2bit(rxMsg2, &spi_data);
00199         can1.read(rxMsg1);                    // read message into Rx message storage
00200         unpack_reply_4bit(rxMsg1, &spi_data);
00201         //wait_us(10);
00202         //wait(0.01);
00203         
00204         }
00205 
00206 }