Aditya Mehrotra
Teleop Demo Code
Diff: main.cpp
- Revision:
- 2:7afc501b8283
- Parent:
- 1:66ff8f8e65f7
- Child:
- 3:ab8084bfcb53
--- a/main.cpp Sat Apr 17 17:49:36 2021 +0000
+++ b/main.cpp Mon Sep 27 02:53:32 2021 +0000
@@ -1,41 +1,22 @@
#include "mbed.h"
#include "crc.h"
+#define WAIT_TIME_MS 5
+#define LEN 100
+
Serial pc(USBTX, USBRX);
-Serial uart(PA_0, PA_1);
-DigitalOut RTS(D12);
-
-//DigitalOut led(LED1);
+RawSerial uart(D1, D0);
+DigitalInOut RTS(D3);
-DigitalOut dbg(D2);
-DigitalOut flip(D3);
-
-AnalogIn GainPot(A2);
-DigitalIn Calibrate(D7);
-DigitalIn GainUpdate(D6);
-
-DigitalOut led(D8);
+DigitalOut dbg(LED1);
+DigitalOut flip(D2);
volatile uint8_t waitForReceive = 0;
volatile uint8_t nextReload = 15;
-uint8_t rx_buffer[100];
+uint8_t rx_buffer[LEN];
-extern "C" void DMA1_Stream2_IRQHandler() {
- DMA1->LIFCR |= (1 << 5); // clear the transfer interrupt flag
- // there is some sort of DMA error generated on each transfer
- // it needs to be cleared before we can re-enable
- DMA1->LIFCR = 0xffffffff; // clear other flags which are confusing and bad
-
- DMA1_Stream2->CR &= ~1; // disable interrupt
- // start the next transfer
- DMA1_Stream2->NDTR = nextReload;
- DMA1_Stream2->M0AR = (uint32_t)rx_buffer;
- DMA1_Stream2->CR |= 1; // enable
- flip = 0;
- waitForReceive = 0;
-}
void setGoalPosition(unsigned char ID, unsigned char p1, unsigned char p2, unsigned char p3, unsigned char p4){
uint8_t elements = 16;
unsigned char packetBuffer[elements];
@@ -65,17 +46,19 @@
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_us(200); // fix this!!!
+ wait_ms(10); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
}
}
-void write_get_position(uint8_t id, uint8_t id2) {
+
+void getPosition(uint8_t id/*, uint8_t id2*/) {
for(int i = 0; i < 100; i++) rx_buffer[i] = 0;
- uint8_t tx_buffer[14];
+ uint8_t elements = 14;
+ unsigned char tx_buffer[elements];
// rx_count = 0;
-
+
tx_buffer[0] = 0xff;
tx_buffer[1] = 0xff;
tx_buffer[2] = 0xfd;
@@ -85,44 +68,64 @@
tx_buffer[6] = 0x00; //length2
tx_buffer[7] = 0x02; //read command
tx_buffer[8] = 0x84; //position identifier
- tx_buffer[9] = 0x00;
- tx_buffer[10] = 0x04;
- tx_buffer[11] = 0x00;
- uint16_t crc = update_crc(0, tx_buffer, 12);
+ tx_buffer[9] = 0x00; //SECOND BYTE OF IDENTIFIER (command)
+ tx_buffer[10] = 0x04; //p1 --> means we want 4 bytes back
+ tx_buffer[11] = 0x00; //p2
+
+ unsigned short crc = update_crc(0, tx_buffer, 12);
+
tx_buffer[12] = crc & 0xff;
tx_buffer[13] = (crc >> 8) & 0xff;
// printf("0x%x 0x%x\r\n", tx_buffer[12], tx_buffer[13]);
- nextReload = 11;
+ //nextReload = 11;
RTS = 1;
waitForReceive = 1;
wait_us(100);
for (int i = 0; i < 14; i++) {
uart.putc(tx_buffer[i]);
}
- wait_us(200);
+ wait_us(350);
RTS = 0;
-
+
- flip = 1;
- while (waitForReceive == 1) {
+ for (int i = 0; i<=14; i++){
+ rx_buffer[i] = uart.getc();
}
+
+}
+
+
+void setGoalCurrent(unsigned char ID, unsigned char p1, unsigned char p2){
+ uint8_t elements = 14;
+ unsigned char packetBuffer[elements];
+ if (uart.writeable() ) {
+ packetBuffer[0] = 0xff;
+ packetBuffer[1] = 0xff;
+ packetBuffer[2] = 0xfd;
+ packetBuffer[3] = 0x00;
+ packetBuffer[4] = ID; //ID
+ packetBuffer[5] = 0x07; //length
+ packetBuffer[6] = 0x00; //length2
+ packetBuffer[7] = 0x03; //write command
+ packetBuffer[8] = 0x66; //Position
+ packetBuffer[9] = 0x00;
+ packetBuffer[10] = p1;
+ packetBuffer[11] = p2;
-// for (int i = 0; i < 15; i++) {
-// pc.printf("0x%x ", rx_buffer[i]);
-// }
-// pc.printf("\r\n");
-
- nextReload = 15;
- //pc.printf("NDTR before send2: %d\r\n", DMA1_Stream2->NDTR);
- waitForReceive = 1;
- setGoalPosition(id2, rx_buffer[9], rx_buffer[10], rx_buffer[11], rx_buffer[12]);
-
- while (waitForReceive == 1) {
- //wait_ms(100);
- //printf("NDTR: %d wfr: %d\r\n", DMA1_Stream2->NDTR, waitForReceive);
+ unsigned short crc = update_crc(0,packetBuffer,12);
+
+ packetBuffer[12] = crc&0x00ff; //CRC1
+ packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
+
+ // Send instruction packet
+ RTS = 1; // Enable Tx / Disable Rx
+ for (int i = 0; i< elements; i++) {
+ uart.putc(packetBuffer[i]);
+ }
+ wait_ms(10); // fix this!!!
+ RTS = 0; // Disable Tx / Enable Rx
}
-
}
@@ -139,7 +142,7 @@
packetBuffer[5] = 0x06; //length
packetBuffer[6] = 0x00; //length2
packetBuffer[7] = 0x03; //write command
- packetBuffer[8] = 0x40; //torque enable
+ packetBuffer[8] = 0x40; //enable current mode
packetBuffer[9] = 0x00;
packetBuffer[10] = enable;
@@ -153,7 +156,7 @@
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_us(200); // fix this!!!
+ wait_ms(WAIT_TIME_MS); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
wait_ms(10);
}
@@ -185,17 +188,15 @@
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_ms(1); // fix this!!!
+ wait_ms(WAIT_TIME_MS); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
wait_ms(10);
}
}
-void setD(unsigned char ID, uint16_t kd) {
-
- uint8_t kd2 = kd >> 8;
- uint8_t kd1 = kd & 0xff;
- uint8_t elements = 14;
+void ledOff(unsigned char ID)
+{
+ uint8_t elements = 13;
unsigned char packetBuffer[elements];
if (uart.writeable() ) {
packetBuffer[0] = 0xff;
@@ -203,34 +204,33 @@
packetBuffer[2] = 0xfd;
packetBuffer[3] = 0x00;
packetBuffer[4] = ID; //ID
- packetBuffer[5] = 0x07; //length
+ packetBuffer[5] = 0x06; //length
packetBuffer[6] = 0x00; //length2
packetBuffer[7] = 0x03; //write command
- packetBuffer[8] = 0x50; //Kp
+ packetBuffer[8] = 0x41; //led
packetBuffer[9] = 0x00;
- packetBuffer[10] = kd1;
- packetBuffer[11] = kd2;
+ packetBuffer[10] = 0x00; //off
- unsigned short crc = update_crc(0,packetBuffer,12);
+ unsigned short crc = update_crc(0,packetBuffer,11);
- packetBuffer[12] = crc&0x00ff; //CRC1
- packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
+ packetBuffer[11] = crc&0x00ff; //CRC1
+ packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
// Send instruction packet
RTS = 1; // Enable Tx / Disable Rx
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_ms(1); // fix this!!!
+ wait_ms(5); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
wait_ms(10);
}
}
-void setP(unsigned char ID, uint16_t kp) {
- uint8_t kp2 = kp >> 8;
- uint8_t kp1 = kp & 0xff;
- uint8_t elements = 14;
-// printf("0x%x 0x%x\r\n", kp1, kp2);
+
+
+void enPosMode(unsigned char ID)
+{
+ uint8_t elements = 13;
unsigned char packetBuffer[elements];
if (uart.writeable() ) {
packetBuffer[0] = 0xff;
@@ -238,34 +238,32 @@
packetBuffer[2] = 0xfd;
packetBuffer[3] = 0x00;
packetBuffer[4] = ID; //ID
- packetBuffer[5] = 0x07; //length
+ packetBuffer[5] = 0x06; //length
packetBuffer[6] = 0x00; //length2
packetBuffer[7] = 0x03; //write command
- packetBuffer[8] = 0x54; //Kp
+ packetBuffer[8] = 0x0B; //mode
packetBuffer[9] = 0x00;
- packetBuffer[10] = kp1;
- packetBuffer[11] = kp2;
+ packetBuffer[10] = 0x03; //on
- unsigned short crc = update_crc(0,packetBuffer,12);
+ unsigned short crc = update_crc(0,packetBuffer,11);
- packetBuffer[12] = crc&0x00ff; //CRC1
- packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
+ packetBuffer[11] = crc&0x00ff; //CRC1
+ packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
// Send instruction packet
RTS = 1; // Enable Tx / Disable Rx
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_ms(1); // fix this!!!
+ wait_ms(WAIT_TIME_MS); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
wait_ms(10);
}
}
-void setFFA(unsigned char ID, uint16_t k) {
- uint8_t kp2 = k >> 8;
- uint8_t kp1 = k & 0xff;
- uint8_t elements = 14;
-// printf("0x%x 0x%x\r\n", kp1, kp2);
+
+void enCurMode(unsigned char ID)
+{
+ uint8_t elements = 13;
unsigned char packetBuffer[elements];
if (uart.writeable() ) {
packetBuffer[0] = 0xff;
@@ -273,34 +271,33 @@
packetBuffer[2] = 0xfd;
packetBuffer[3] = 0x00;
packetBuffer[4] = ID; //ID
- packetBuffer[5] = 0x07; //length
+ packetBuffer[5] = 0x06; //length
packetBuffer[6] = 0x00; //length2
packetBuffer[7] = 0x03; //write command
- packetBuffer[8] = 0x58; //Feed forward acceleration
+ packetBuffer[8] = 0x0B; //mode
packetBuffer[9] = 0x00;
- packetBuffer[10] = kp1;
- packetBuffer[11] = kp2;
+ packetBuffer[10] = 0x00; //on
- unsigned short crc = update_crc(0,packetBuffer,12);
+ unsigned short crc = update_crc(0,packetBuffer,11);
- packetBuffer[12] = crc&0x00ff; //CRC1
- packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
+ packetBuffer[11] = crc&0x00ff; //CRC1
+ packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
// Send instruction packet
RTS = 1; // Enable Tx / Disable Rx
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_ms(1); // fix this!!!
+ wait_ms(WAIT_TIME_MS); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
wait_ms(10);
}
}
-void setFFV(unsigned char ID, uint16_t k) {
- uint8_t kp2 = k >> 8;
- uint8_t kp1 = k & 0xff;
- uint8_t elements = 14;
-// printf("0x%x 0x%x\r\n", kp1, kp2);
+
+
+void setBaud(unsigned char ID, unsigned char RATE)
+{
+ uint8_t elements = 13;
unsigned char packetBuffer[elements];
if (uart.writeable() ) {
packetBuffer[0] = 0xff;
@@ -308,91 +305,87 @@
packetBuffer[2] = 0xfd;
packetBuffer[3] = 0x00;
packetBuffer[4] = ID; //ID
- packetBuffer[5] = 0x07; //length
+ packetBuffer[5] = 0x06; //length
packetBuffer[6] = 0x00; //length2
packetBuffer[7] = 0x03; //write command
- packetBuffer[8] = 0x5A; //Feed forward velocity
+ packetBuffer[8] = 0x08; //baud rate
packetBuffer[9] = 0x00;
- packetBuffer[10] = kp1;
- packetBuffer[11] = kp2;
+ packetBuffer[10] = RATE; //0x01 - 57600, 0x02 - 115,200, 0x03 - 1000000
- unsigned short crc = update_crc(0,packetBuffer,12);
+ unsigned short crc = update_crc(0,packetBuffer,11);
- packetBuffer[12] = crc&0x00ff; //CRC1
- packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
+ packetBuffer[11] = crc&0x00ff; //CRC1
+ packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
// Send instruction packet
RTS = 1; // Enable Tx / Disable Rx
for (int i = 0; i< elements; i++) {
uart.putc(packetBuffer[i]);
}
- wait_ms(1); // fix this!!!
+ wait_ms(5); // fix this!!!
RTS = 0; // Disable Tx / Enable Rx
wait_ms(10);
}
}
+
+
+
+
+
int main()
{
wait_ms(300);
RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;
+ RTS.mode(OpenDrainNoPull);
+ RTS.output();
pc.baud(115200);
- uart.baud(1000000);
-// uart.attach(rxCallback);
- UART4->CR3 |= USART_CR3_DMAR;
- DMA1_Stream2->PAR = (uint32_t)&UART4->DR;
- DMA1_Stream2->M0AR = (uint32_t)rx_buffer;
- DMA1_Stream2->CR |= (4<<25);
- DMA1_Stream2->NDTR = 16;
-// DMA1_Stream2->CR |= DMA_SxCR_PFCTRL;
- DMA1_Stream2->CR |= DMA_SxCR_MINC;
- NVIC_EnableIRQ(DMA1_Stream2_IRQn);
- DMA1_Stream2->CR |= DMA_SxCR_TCIE;
- pc.printf("Enabling DMA\r\n");
- //DMA1_Stream2->CR |= DMA_SxCR_EN;
- pc.printf("DMA Enabled\r\n");
+ uart.baud(57600);
+ RTS = 0;
+ wait(1);
+
+ //setBaud(0x01, 0x01);
+ //wait(1);
+
pc.printf("Program is running!\r\n");
-
+ //torqueEnable(0x01,0x01);
+ //wait(1);
- ledOn(0x01);
- setD(0x01, 120);
- setP(0x01,200);
- setD(0x02, 120);
- setP(0x02, 200);
- setD(0x03, 0);
- setD(0x04, 0);
- setD(0x05, 0);
- setD(0x06, 0);
- setP(0x03, 200);
- setP(0x04, 200);
- setP(0x05, 200);
- setP(0x06, 200);
-// setFFA(0x03, 800);
-// setFFA(0x05, 800);
- //setGoalPosition(0x04, 0xD0, 0x07, 0x00, 0x00);
-// wait_ms(10);
- torqueEnable(0x02,0x01);
+ getPosition(0x01);
+ wait(1);
+
+ //torqueEnable(0x01,0x00);
+ //wait(1);
+
+ for (int i = 0; i<=14; i++) {
+ pc.printf("%hx ", rx_buffer[i]); //USE %HX FOR HEX!!!
+ }
+ pc.printf("\n");
+ pc.printf("done!n");
+
+
+
+
+ /*
+ ledOff(0x01);
+ torqueEnable(0x01,0x00);
+ enPosMode(0x01);
+ //enCurMode(0x01);
+ wait(1);
+ ledOn(0x01);
+ //setP(0x01,100);
torqueEnable(0x01,0x01);
- torqueEnable(0x03,0x01);
- torqueEnable(0x04,0x01);
- torqueEnable(0x05,0x01);
- torqueEnable(0x06,0x01);
wait(1);
- led = 1;
- while(uart.readable()) {uart.getc();}
- DMA1_Stream2->CR |= DMA_SxCR_EN;
- for (;;) {
-
- dbg = 1;
-// ledOn(0x03);
- write_get_position(0x01,0x02);
- write_get_position(0x02,0x01);
- write_get_position(0x03,0x04);
- write_get_position(0x04,0x03);
- write_get_position(0x05,0x06);
- write_get_position(0x06,0x05);
- //wait_ms(10);
- dbg = 0;
- }
+ //setGoalCurrent(0x01, 0xF0, 0x00);
+ setGoalPosition(0x01,0x00,0x0F,0x00,0x00);
+ wait(5);
+ ledOff(0x01);
+ torqueEnable(0x01,0x00);
+ //led = 1;
+ */
+
+
+
+
}