Diff: main.cpp

Revision:

0:58d3ade33116

Child:

1:542050a917de

diff -r 000000000000 -r 58d3ade33116 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 19 19:55:51 2016 +0000
@@ -0,0 +1,145 @@
+#include "mbed.h"
+
+Serial motorDriver(PA_9, PA_10);
+//Serial pc(PA_2, PA_3);
+
+
+SPISlave spi(PA_7, PA_6, PA_5, PA_4);
+
+
+
+
+
+
+Ticker loop;
+int response1 = 0;
+
+int count = 0;
+int bytes[4];
+
+int encoder = 0;
+int current = 0;
+int flag = 0;
+int currentCmd = 0;
+int currentCmdOld = 0;
+
+
+
+void setCurrent(int current){
+    wait_us(5);
+    int tc;
+    if (current<-2048){ current = -2048;}   //1800 = 60A = 27 Nm
+    else if (current>2047){ current = 2047;}
+
+    tc = current;
+    char byte1 = 0x40+ ((tc&0xFFF)>>6);
+    char  byte2 = tc&0x3F;
+
+    motorDriver.putc(byte1);
+    motorDriver.putc(byte2);
+    
+    //pc.printf("tc: %d   byte1: %d    byte2: %d\n\r", tc, byte1, byte2);
+    
+    }
+
+void sendCmd(void){
+    //motorDriver.putc(0x02);
+    //motorDriver.putc(0x00);
+    //motorDriver.putc(0x61);
+    setCurrent(currentCmd);
+    //pc.printf("%d\n\r", currentCmd);
+    count = 1;
+    }
+    
+
+    
+void readSPI(void){
+    //if(spi.receive()){
+    wait_us(1);
+    response1 = SPI1->DR;   // read response straight out of the register.  spi.read() breaks everything, not sure why.
+    //printf('%d\n\r', currentCmd);
+        spi.reply(encoder+28672);
+        //printf("%d\n\r", response1);
+        currentCmd = response1-32768;
+        
+        if(currentCmd < -2048){currentCmd = currentCmdOld;}     //throw away out-of-bounds values
+        else if(currentCmd > 2048){currentCmd = currentCmdOld;}
+        currentCmdOld = currentCmd;
+        
+        sendCmd();
+        //pc.printf("%d\n\r", currentCmd);
+   //   }
+    }
+
+ extern "C" void SPI1_IRQHandler(void)
+{
+    readSPI();
+
+}
+   
+void echo(void){
+    
+    while(motorDriver.readable()){
+    int byte = motorDriver.getc();
+    //pc.putc(byte);
+    if(count>0){
+        bytes[count-1] = byte;
+        if(count == 4){
+             encoder = (bytes[0]<<5) + (bytes[1]>>3);
+             current = ((bytes[1]&0x7)<<9) + (bytes[2]<<1) + (bytes[3]>>7);
+             flag = bytes[3]&0x7F;
+             count = 0;
+             
+             //pc.printf("  Encoder: %d    Current: %d    Flag:  %d\n\r",encoder, current, flag);
+             }
+        else{count++;}
+        }
+
+    //count++;
+        }
+    }
+    
+void transmit(void){
+    //motorDriver.putc(pc.getc());
+    }
+  
+
+
+void printStuff(void){
+//    pc.printf("%d\n\r", currentCmd);
+
+    }
+    
+
+    
+int main() {
+//    pc.baud(1000000);
+//    pc.attach(&transmit);
+//    pc.printf("SPI Dongle\n\r");
+    
+    
+    spi.format(16, 0);
+    spi.frequency(12000000);
+    spi.reply(0x0);
+
+    
+    motorDriver.baud(1000000);
+    motorDriver.attach(&echo);
+    motorDriver.format(8, Serial::None, 2);
+    wait(.1);
+    motorDriver.putc(0x80);
+    wait(.1);
+    motorDriver.putc(0x6d);
+    
+    
+    
+
+    while(1) {
+
+    readSPI();
+    wait_us(50);
+    //        }
+
+     
+    }
+}