Patrick Wensing / Mbed 2 deprecated Cheetah3Passthrough

passthrough w updated timing

Dependencies:   mbed

Fork of Cheetah3Passthrough by Ben Katz

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?

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
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 }