Patrick Wensing / Mbed 2 deprecated Cheetah3Passthrough

passthrough w updated timing

Dependencies:   mbed

Fork of Cheetah3Passthrough by Ben Katz

main.cpp@0:58d3ade33116, 2016-12-19 (annotated)

Committer:
benkatz
Date:
Mon Dec 19 19:55:51 2016 +0000
Revision:
0:58d3ade33116
Child:
1:542050a917de
Cheetah 3 Passthrough Firmware

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:58d3ade33116 1 #include "mbed.h"
benkatz 0:58d3ade33116 2
benkatz 0:58d3ade33116 3 Serial motorDriver(PA_9, PA_10);
benkatz 0:58d3ade33116 4 //Serial pc(PA_2, PA_3);
benkatz 0:58d3ade33116 5
benkatz 0:58d3ade33116 6
benkatz 0:58d3ade33116 7 SPISlave spi(PA_7, PA_6, PA_5, PA_4);
benkatz 0:58d3ade33116 8
benkatz 0:58d3ade33116 9
benkatz 0:58d3ade33116 10
benkatz 0:58d3ade33116 11
benkatz 0:58d3ade33116 12
benkatz 0:58d3ade33116 13
benkatz 0:58d3ade33116 14 Ticker loop;
benkatz 0:58d3ade33116 15 int response1 = 0;
benkatz 0:58d3ade33116 16
benkatz 0:58d3ade33116 17 int count = 0;
benkatz 0:58d3ade33116 18 int bytes[4];
benkatz 0:58d3ade33116 19
benkatz 0:58d3ade33116 20 int encoder = 0;
benkatz 0:58d3ade33116 21 int current = 0;
benkatz 0:58d3ade33116 22 int flag = 0;
benkatz 0:58d3ade33116 23 int currentCmd = 0;
benkatz 0:58d3ade33116 24 int currentCmdOld = 0;
benkatz 0:58d3ade33116 25
benkatz 0:58d3ade33116 26
benkatz 0:58d3ade33116 27
benkatz 0:58d3ade33116 28 void setCurrent(int current){
benkatz 0:58d3ade33116 29 wait_us(5);
benkatz 0:58d3ade33116 30 int tc;
benkatz 0:58d3ade33116 31 if (current<-2048){ current = -2048;} //1800 = 60A = 27 Nm
benkatz 0:58d3ade33116 32 else if (current>2047){ current = 2047;}
benkatz 0:58d3ade33116 33
benkatz 0:58d3ade33116 34 tc = current;
benkatz 0:58d3ade33116 35 char byte1 = 0x40+ ((tc&0xFFF)>>6);
benkatz 0:58d3ade33116 36 char byte2 = tc&0x3F;
benkatz 0:58d3ade33116 37
benkatz 0:58d3ade33116 38 motorDriver.putc(byte1);
benkatz 0:58d3ade33116 39 motorDriver.putc(byte2);
benkatz 0:58d3ade33116 40
benkatz 0:58d3ade33116 41 //pc.printf("tc: %d byte1: %d byte2: %d\n\r", tc, byte1, byte2);
benkatz 0:58d3ade33116 42
benkatz 0:58d3ade33116 43 }
benkatz 0:58d3ade33116 44
benkatz 0:58d3ade33116 45 void sendCmd(void){
benkatz 0:58d3ade33116 46 //motorDriver.putc(0x02);
benkatz 0:58d3ade33116 47 //motorDriver.putc(0x00);
benkatz 0:58d3ade33116 48 //motorDriver.putc(0x61);
benkatz 0:58d3ade33116 49 setCurrent(currentCmd);
benkatz 0:58d3ade33116 50 //pc.printf("%d\n\r", currentCmd);
benkatz 0:58d3ade33116 51 count = 1;
benkatz 0:58d3ade33116 52 }
benkatz 0:58d3ade33116 53
benkatz 0:58d3ade33116 54
benkatz 0:58d3ade33116 55
benkatz 0:58d3ade33116 56 void readSPI(void){
benkatz 0:58d3ade33116 57 //if(spi.receive()){
benkatz 0:58d3ade33116 58 wait_us(1);
benkatz 0:58d3ade33116 59 response1 = SPI1->DR; // read response straight out of the register. spi.read() breaks everything, not sure why.
benkatz 0:58d3ade33116 60 //printf('%d\n\r', currentCmd);
benkatz 0:58d3ade33116 61 spi.reply(encoder+28672);
benkatz 0:58d3ade33116 62 //printf("%d\n\r", response1);
benkatz 0:58d3ade33116 63 currentCmd = response1-32768;
benkatz 0:58d3ade33116 64
benkatz 0:58d3ade33116 65 if(currentCmd < -2048){currentCmd = currentCmdOld;} //throw away out-of-bounds values
benkatz 0:58d3ade33116 66 else if(currentCmd > 2048){currentCmd = currentCmdOld;}
benkatz 0:58d3ade33116 67 currentCmdOld = currentCmd;
benkatz 0:58d3ade33116 68
benkatz 0:58d3ade33116 69 sendCmd();
benkatz 0:58d3ade33116 70 //pc.printf("%d\n\r", currentCmd);
benkatz 0:58d3ade33116 71 // }
benkatz 0:58d3ade33116 72 }
benkatz 0:58d3ade33116 73
benkatz 0:58d3ade33116 74 extern "C" void SPI1_IRQHandler(void)
benkatz 0:58d3ade33116 75 {
benkatz 0:58d3ade33116 76 readSPI();
benkatz 0:58d3ade33116 77
benkatz 0:58d3ade33116 78 }
benkatz 0:58d3ade33116 79
benkatz 0:58d3ade33116 80 void echo(void){
benkatz 0:58d3ade33116 81
benkatz 0:58d3ade33116 82 while(motorDriver.readable()){
benkatz 0:58d3ade33116 83 int byte = motorDriver.getc();
benkatz 0:58d3ade33116 84 //pc.putc(byte);
benkatz 0:58d3ade33116 85 if(count>0){
benkatz 0:58d3ade33116 86 bytes[count-1] = byte;
benkatz 0:58d3ade33116 87 if(count == 4){
benkatz 0:58d3ade33116 88 encoder = (bytes[0]<<5) + (bytes[1]>>3);
benkatz 0:58d3ade33116 89 current = ((bytes[1]&0x7)<<9) + (bytes[2]<<1) + (bytes[3]>>7);
benkatz 0:58d3ade33116 90 flag = bytes[3]&0x7F;
benkatz 0:58d3ade33116 91 count = 0;
benkatz 0:58d3ade33116 92
benkatz 0:58d3ade33116 93 //pc.printf(" Encoder: %d Current: %d Flag: %d\n\r",encoder, current, flag);
benkatz 0:58d3ade33116 94 }
benkatz 0:58d3ade33116 95 else{count++;}
benkatz 0:58d3ade33116 96 }
benkatz 0:58d3ade33116 97
benkatz 0:58d3ade33116 98 //count++;
benkatz 0:58d3ade33116 99 }
benkatz 0:58d3ade33116 100 }
benkatz 0:58d3ade33116 101
benkatz 0:58d3ade33116 102 void transmit(void){
benkatz 0:58d3ade33116 103 //motorDriver.putc(pc.getc());
benkatz 0:58d3ade33116 104 }
benkatz 0:58d3ade33116 105
benkatz 0:58d3ade33116 106
benkatz 0:58d3ade33116 107
benkatz 0:58d3ade33116 108 void printStuff(void){
benkatz 0:58d3ade33116 109 // pc.printf("%d\n\r", currentCmd);
benkatz 0:58d3ade33116 110
benkatz 0:58d3ade33116 111 }
benkatz 0:58d3ade33116 112
benkatz 0:58d3ade33116 113
benkatz 0:58d3ade33116 114
benkatz 0:58d3ade33116 115 int main() {
benkatz 0:58d3ade33116 116 // pc.baud(1000000);
benkatz 0:58d3ade33116 117 // pc.attach(&transmit);
benkatz 0:58d3ade33116 118 // pc.printf("SPI Dongle\n\r");
benkatz 0:58d3ade33116 119
benkatz 0:58d3ade33116 120
benkatz 0:58d3ade33116 121 spi.format(16, 0);
benkatz 0:58d3ade33116 122 spi.frequency(12000000);
benkatz 0:58d3ade33116 123 spi.reply(0x0);
benkatz 0:58d3ade33116 124
benkatz 0:58d3ade33116 125
benkatz 0:58d3ade33116 126 motorDriver.baud(1000000);
benkatz 0:58d3ade33116 127 motorDriver.attach(&echo);
benkatz 0:58d3ade33116 128 motorDriver.format(8, Serial::None, 2);
benkatz 0:58d3ade33116 129 wait(.1);
benkatz 0:58d3ade33116 130 motorDriver.putc(0x80);
benkatz 0:58d3ade33116 131 wait(.1);
benkatz 0:58d3ade33116 132 motorDriver.putc(0x6d);
benkatz 0:58d3ade33116 133
benkatz 0:58d3ade33116 134
benkatz 0:58d3ade33116 135
benkatz 0:58d3ade33116 136
benkatz 0:58d3ade33116 137 while(1) {
benkatz 0:58d3ade33116 138
benkatz 0:58d3ade33116 139 readSPI();
benkatz 0:58d3ade33116 140 wait_us(50);
benkatz 0:58d3ade33116 141 // }
benkatz 0:58d3ade33116 142
benkatz 0:58d3ade33116 143
benkatz 0:58d3ade33116 144 }
benkatz 0:58d3ade33116 145 }