Patrick Wensing
/
Cheetah3Passthrough
passthrough w updated timing
Fork of Cheetah3Passthrough by
main.cpp@1:542050a917de, 2016-12-27 (annotated)
- Committer:
- pwensing
- Date:
- Tue Dec 27 00:33:53 2016 +0000
- Revision:
- 1:542050a917de
- Parent:
- 0:58d3ade33116
updated timing;
Who changed what in which revision?
User | Revision | Line number | New 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 | |
pwensing | 1:542050a917de | 10 | void flush() |
pwensing | 1:542050a917de | 11 | { |
pwensing | 1:542050a917de | 12 | while(motorDriver.readable()){ |
pwensing | 1:542050a917de | 13 | int byte = motorDriver.getc(); |
pwensing | 1:542050a917de | 14 | } |
pwensing | 1:542050a917de | 15 | } |
benkatz | 0:58d3ade33116 | 16 | |
benkatz | 0:58d3ade33116 | 17 | |
benkatz | 0:58d3ade33116 | 18 | |
benkatz | 0:58d3ade33116 | 19 | |
benkatz | 0:58d3ade33116 | 20 | Ticker loop; |
benkatz | 0:58d3ade33116 | 21 | int response1 = 0; |
benkatz | 0:58d3ade33116 | 22 | |
benkatz | 0:58d3ade33116 | 23 | int count = 0; |
benkatz | 0:58d3ade33116 | 24 | int bytes[4]; |
benkatz | 0:58d3ade33116 | 25 | |
benkatz | 0:58d3ade33116 | 26 | int encoder = 0; |
benkatz | 0:58d3ade33116 | 27 | int current = 0; |
benkatz | 0:58d3ade33116 | 28 | int flag = 0; |
benkatz | 0:58d3ade33116 | 29 | int currentCmd = 0; |
benkatz | 0:58d3ade33116 | 30 | int currentCmdOld = 0; |
benkatz | 0:58d3ade33116 | 31 | |
benkatz | 0:58d3ade33116 | 32 | |
benkatz | 0:58d3ade33116 | 33 | |
benkatz | 0:58d3ade33116 | 34 | void setCurrent(int current){ |
benkatz | 0:58d3ade33116 | 35 | wait_us(5); |
benkatz | 0:58d3ade33116 | 36 | int tc; |
benkatz | 0:58d3ade33116 | 37 | if (current<-2048){ current = -2048;} //1800 = 60A = 27 Nm |
benkatz | 0:58d3ade33116 | 38 | else if (current>2047){ current = 2047;} |
benkatz | 0:58d3ade33116 | 39 | |
benkatz | 0:58d3ade33116 | 40 | tc = current; |
benkatz | 0:58d3ade33116 | 41 | char byte1 = 0x40+ ((tc&0xFFF)>>6); |
benkatz | 0:58d3ade33116 | 42 | char byte2 = tc&0x3F; |
benkatz | 0:58d3ade33116 | 43 | |
benkatz | 0:58d3ade33116 | 44 | motorDriver.putc(byte1); |
benkatz | 0:58d3ade33116 | 45 | motorDriver.putc(byte2); |
benkatz | 0:58d3ade33116 | 46 | |
benkatz | 0:58d3ade33116 | 47 | //pc.printf("tc: %d byte1: %d byte2: %d\n\r", tc, byte1, byte2); |
benkatz | 0:58d3ade33116 | 48 | |
benkatz | 0:58d3ade33116 | 49 | } |
benkatz | 0:58d3ade33116 | 50 | |
benkatz | 0:58d3ade33116 | 51 | void sendCmd(void){ |
benkatz | 0:58d3ade33116 | 52 | //motorDriver.putc(0x02); |
benkatz | 0:58d3ade33116 | 53 | //motorDriver.putc(0x00); |
benkatz | 0:58d3ade33116 | 54 | //motorDriver.putc(0x61); |
benkatz | 0:58d3ade33116 | 55 | setCurrent(currentCmd); |
benkatz | 0:58d3ade33116 | 56 | //pc.printf("%d\n\r", currentCmd); |
benkatz | 0:58d3ade33116 | 57 | count = 1; |
benkatz | 0:58d3ade33116 | 58 | } |
benkatz | 0:58d3ade33116 | 59 | |
benkatz | 0:58d3ade33116 | 60 | |
benkatz | 0:58d3ade33116 | 61 | |
benkatz | 0:58d3ade33116 | 62 | void readSPI(void){ |
benkatz | 0:58d3ade33116 | 63 | //if(spi.receive()){ |
benkatz | 0:58d3ade33116 | 64 | wait_us(1); |
benkatz | 0:58d3ade33116 | 65 | response1 = SPI1->DR; // read response straight out of the register. spi.read() breaks everything, not sure why. |
benkatz | 0:58d3ade33116 | 66 | //printf('%d\n\r', currentCmd); |
benkatz | 0:58d3ade33116 | 67 | spi.reply(encoder+28672); |
benkatz | 0:58d3ade33116 | 68 | //printf("%d\n\r", response1); |
benkatz | 0:58d3ade33116 | 69 | currentCmd = response1-32768; |
benkatz | 0:58d3ade33116 | 70 | |
benkatz | 0:58d3ade33116 | 71 | if(currentCmd < -2048){currentCmd = currentCmdOld;} //throw away out-of-bounds values |
benkatz | 0:58d3ade33116 | 72 | else if(currentCmd > 2048){currentCmd = currentCmdOld;} |
benkatz | 0:58d3ade33116 | 73 | currentCmdOld = currentCmd; |
benkatz | 0:58d3ade33116 | 74 | |
benkatz | 0:58d3ade33116 | 75 | sendCmd(); |
benkatz | 0:58d3ade33116 | 76 | //pc.printf("%d\n\r", currentCmd); |
benkatz | 0:58d3ade33116 | 77 | // } |
benkatz | 0:58d3ade33116 | 78 | } |
benkatz | 0:58d3ade33116 | 79 | |
benkatz | 0:58d3ade33116 | 80 | extern "C" void SPI1_IRQHandler(void) |
benkatz | 0:58d3ade33116 | 81 | { |
benkatz | 0:58d3ade33116 | 82 | readSPI(); |
benkatz | 0:58d3ade33116 | 83 | |
benkatz | 0:58d3ade33116 | 84 | } |
benkatz | 0:58d3ade33116 | 85 | |
benkatz | 0:58d3ade33116 | 86 | void echo(void){ |
benkatz | 0:58d3ade33116 | 87 | |
benkatz | 0:58d3ade33116 | 88 | while(motorDriver.readable()){ |
benkatz | 0:58d3ade33116 | 89 | int byte = motorDriver.getc(); |
benkatz | 0:58d3ade33116 | 90 | //pc.putc(byte); |
benkatz | 0:58d3ade33116 | 91 | if(count>0){ |
benkatz | 0:58d3ade33116 | 92 | bytes[count-1] = byte; |
benkatz | 0:58d3ade33116 | 93 | if(count == 4){ |
benkatz | 0:58d3ade33116 | 94 | encoder = (bytes[0]<<5) + (bytes[1]>>3); |
benkatz | 0:58d3ade33116 | 95 | current = ((bytes[1]&0x7)<<9) + (bytes[2]<<1) + (bytes[3]>>7); |
benkatz | 0:58d3ade33116 | 96 | flag = bytes[3]&0x7F; |
benkatz | 0:58d3ade33116 | 97 | count = 0; |
benkatz | 0:58d3ade33116 | 98 | |
benkatz | 0:58d3ade33116 | 99 | //pc.printf(" Encoder: %d Current: %d Flag: %d\n\r",encoder, current, flag); |
benkatz | 0:58d3ade33116 | 100 | } |
benkatz | 0:58d3ade33116 | 101 | else{count++;} |
benkatz | 0:58d3ade33116 | 102 | } |
benkatz | 0:58d3ade33116 | 103 | |
benkatz | 0:58d3ade33116 | 104 | //count++; |
benkatz | 0:58d3ade33116 | 105 | } |
benkatz | 0:58d3ade33116 | 106 | } |
benkatz | 0:58d3ade33116 | 107 | |
benkatz | 0:58d3ade33116 | 108 | void transmit(void){ |
benkatz | 0:58d3ade33116 | 109 | //motorDriver.putc(pc.getc()); |
benkatz | 0:58d3ade33116 | 110 | } |
benkatz | 0:58d3ade33116 | 111 | |
benkatz | 0:58d3ade33116 | 112 | |
benkatz | 0:58d3ade33116 | 113 | |
benkatz | 0:58d3ade33116 | 114 | void printStuff(void){ |
benkatz | 0:58d3ade33116 | 115 | // pc.printf("%d\n\r", currentCmd); |
benkatz | 0:58d3ade33116 | 116 | |
benkatz | 0:58d3ade33116 | 117 | } |
benkatz | 0:58d3ade33116 | 118 | |
benkatz | 0:58d3ade33116 | 119 | |
benkatz | 0:58d3ade33116 | 120 | |
benkatz | 0:58d3ade33116 | 121 | int main() { |
benkatz | 0:58d3ade33116 | 122 | // pc.baud(1000000); |
benkatz | 0:58d3ade33116 | 123 | // pc.attach(&transmit); |
benkatz | 0:58d3ade33116 | 124 | // pc.printf("SPI Dongle\n\r"); |
benkatz | 0:58d3ade33116 | 125 | |
benkatz | 0:58d3ade33116 | 126 | |
benkatz | 0:58d3ade33116 | 127 | spi.format(16, 0); |
benkatz | 0:58d3ade33116 | 128 | spi.frequency(12000000); |
benkatz | 0:58d3ade33116 | 129 | spi.reply(0x0); |
benkatz | 0:58d3ade33116 | 130 | |
benkatz | 0:58d3ade33116 | 131 | |
pwensing | 1:542050a917de | 132 | |
benkatz | 0:58d3ade33116 | 133 | motorDriver.baud(1000000); |
benkatz | 0:58d3ade33116 | 134 | motorDriver.attach(&echo); |
benkatz | 0:58d3ade33116 | 135 | motorDriver.format(8, Serial::None, 2); |
pwensing | 1:542050a917de | 136 | wait(.4); |
pwensing | 1:542050a917de | 137 | motorDriver.putc(0x80); |
pwensing | 1:542050a917de | 138 | wait(.2); |
benkatz | 0:58d3ade33116 | 139 | motorDriver.putc(0x80); |
pwensing | 1:542050a917de | 140 | wait(.2); |
pwensing | 1:542050a917de | 141 | motorDriver.putc('p'); |
pwensing | 1:542050a917de | 142 | wait(.2); |
pwensing | 1:542050a917de | 143 | motorDriver.putc('m'); |
pwensing | 1:542050a917de | 144 | wait(.2); |
pwensing | 1:542050a917de | 145 | motorDriver.putc(0x40); |
pwensing | 1:542050a917de | 146 | motorDriver.putc(0x00); |
benkatz | 0:58d3ade33116 | 147 | |
benkatz | 0:58d3ade33116 | 148 | |
benkatz | 0:58d3ade33116 | 149 | while(1) { |
benkatz | 0:58d3ade33116 | 150 | |
benkatz | 0:58d3ade33116 | 151 | readSPI(); |
benkatz | 0:58d3ade33116 | 152 | wait_us(50); |
benkatz | 0:58d3ade33116 | 153 | // } |
benkatz | 0:58d3ade33116 | 154 | |
benkatz | 0:58d3ade33116 | 155 | |
benkatz | 0:58d3ade33116 | 156 | } |
benkatz | 0:58d3ade33116 | 157 | } |