Merge branch 'newstuff' of github.com:rene-dev/stmbl into newstuff

2026-09-22 06:44:03 +08:00 · 2016-09-04 02:00:16 +02:00
parent 66f6eaca28 eef35bd282
commit f46cb83cf1
3 changed files with 244 additions and 5 deletions
@@ -11,12 +11,10 @@

 #define DATABAUD 2250000 //baudrate used for communication

-//fixed point calculations
+//fixed point calculations signed bit, 9 bit predecimal, 6 bit decimal
 #define TOFIXED(a) ((int16_t)((a) * 64))
 #define TOFLOAT(a) ((float)((a) / 64.0))

-#define TOSFLOAT(a) (ABS(a) >= 4.0) ? (((int16_t)((a) * 64)) | 0x4000) : (((int16_t)((a) * 4096.0)) & 0xBFFFF)
-#define TOFLOATS(a) ((a) & 0x4000) ? ((float)(((a) & 0xBFFFF) / 64.0)) : ((float)(((a) & 0xBFFFF) / 4096.0))

 #define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
 #define MIN(a, b)  (((a) < (b)) ? (a) : (b))
@@ -0,0 +1,239 @@
+/*
+* This file is part of the stmbl project.
+*
+* Copyright (C) 2016 Rene Hopf <renehopf@mac.com>
+*
+* This program is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+HAL_COMP(netbob);
+
+// pins
+HAL_PIN(dump_pd_vals) = 0.0;
+HAL_PIN(error) = 0.0;//counts unknown commands
+HAL_PIN(crc_error) = 0.0;//counts crc errors
+HAL_PIN(connected) = 0.0;//connection status TODO: not stable during startup, needs link to pd
+HAL_PIN(timeout) = 100.0;// 20khz / 1khz * 2 reads = 40
+HAL_PIN(rxfoo) = 0.0;
+
+HAL_PIN(pos_cmd) = 0.0;
+HAL_PIN(pos_cmd_d) = 0.0;
+HAL_PIN(pos_fb) = 0.0;
+
+HAL_PIN(in0) = 0.0;
+HAL_PIN(in1) = 0.0;
+HAL_PIN(in2) = 0.0;
+HAL_PIN(in3) = 0.0;
+HAL_PIN(fault) = 0.0;
+
+HAL_PIN(out0) = 0.0;
+HAL_PIN(out1) = 0.0;
+HAL_PIN(out2) = 0.0;
+HAL_PIN(out3) = 0.0;
+HAL_PIN(enable) = 0.0;
+
+#define DRV_COUNT 6
+
+typedef struct{
+   uint8_t sink;
+   uint8_t addr;
+   uint8_t data;
+   struct {
+      uint8_t enable : 1;
+      uint8_t brake : 1;
+      uint8_t tmp : 2;
+      uint8_t io : 4;
+   } flags;
+   float pos;
+   float vel;
+} p_data_t;
+
+typedef struct{
+   uint8_t packed_type;
+   uint8_t length;
+   uint8_t packed_cnt;
+   uint8_t source;
+   p_data_t drive[DRV_COUNT];
+   uint32_t crc;
+} packet_t;
+
+MEM(volatile uint8_t rxbuf[128]);
+MEM(volatile uint8_t txbuf[20]);
+
+MEM(uint16_t address);//current address pointer
+MEM(int rxpos);
+MEM(uint32_t timeout);
+MEM(int bufferpos);
+MEM(int available);
+MEM(uint8_t cnt) = 0;
+
+//pb13 txen
+//pc12 UART5 tx
+//pd2  UART5 rx
+void init_hardware() {
+  GPIO_InitTypeDef GPIO_InitStruct;
+  USART_InitTypeDef USART_InitStruct;
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
+  //USART TX
+  GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5);
+  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12;
+  GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP ;
+  GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  //USART RX
+  GPIO_PinAFConfig(GPIOD, GPIO_PinSource2, GPIO_AF_UART5);
+  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2;
+  GPIO_Init(GPIOD, &GPIO_InitStruct);
+
+  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  USART_InitStruct.USART_BaudRate = 5000000;
+  USART_InitStruct.USART_WordLength = USART_WordLength_8b;
+  USART_InitStruct.USART_StopBits = USART_StopBits_1;
+  USART_InitStruct.USART_Parity = USART_Parity_No;
+  USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+  USART_InitStruct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+  USART_OverSampling8Cmd(UART5,ENABLE);//16 bit: 2.625, 8bit:5.25
+  USART_Init(UART5, &USART_InitStruct);
+
+  USART_Cmd(UART5, ENABLE);
+
+  //RX DMA
+
+  DMA_Cmd(DMA1_Stream0, DISABLE);
+  DMA_DeInit(DMA1_Stream0);
+
+  // DMA1-Config
+  DMA_InitStructure.DMA_Channel = DMA_Channel_4;
+  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->DR);
+  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&rxbuf;
+  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
+  DMA_InitStructure.DMA_BufferSize = sizeof(rxbuf);
+  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+  DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
+  DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
+  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+  DMA_Init(DMA1_Stream0, &DMA_InitStructure);
+
+  DMA_Cmd(DMA1_Stream0, ENABLE);
+
+  USART_DMACmd(UART5, USART_DMAReq_Rx, ENABLE);
+
+  //TX DMA
+
+  DMA_Cmd(DMA1_Stream7, DISABLE);
+  DMA_DeInit(DMA1_Stream7);
+
+  // DMA2-Config
+  DMA_InitStructure.DMA_Channel = DMA_Channel_4;
+  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->DR);
+  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&txbuf;
+  DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+  DMA_InitStructure.DMA_BufferSize = sizeof(txbuf);
+  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+  DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
+  DMA_InitStructure.DMA_Mode =  DMA_Priority_Low;
+  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
+  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+  DMA_Init(DMA1_Stream7, &DMA_InitStructure);
+
+  //DMA_Cmd(DMA1_Stream7, ENABLE);
+
+  USART_DMACmd(UART5, USART_DMAReq_Tx, ENABLE);
+  
+  //tx enable
+  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+   
+  GPIO_ResetBits(GPIOB, GPIO_Pin_13);
+}
+
+
+RT_INIT(
+  init_hardware();
+  rxpos = 0;
+  timeout = 1000;//make sure we start in timeout
+);
+
+
+RT (
+   packet_t packet;
+   //next received packet will be written to bufferpos
+   bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA1_Stream0);
+   //how many packets we have the the rx buffer for processing
+   available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf);
+   if(!EDGE(available)){
+      
+      if(available == sizeof(packet)){
+         //copy packet from dma buffer to struct. TODO: zero copy by writing directly to struct.
+         for(int i = 0;i<sizeof(packet);i++){
+            ((uint8_t*)&packet)[i] = rxbuf[(rxpos + i)%sizeof(rxbuf)];
+         }
+         
+         //check crc
+         CRC_ResetDR();
+         if(packet.crc != CRC_CalcBlockCRC((uint32_t *) &packet, sizeof(packet) / 4 - 1)){
+            PIN(crc_error)++;
+         }else{
+            //check for lost packets
+            if(packet.packed_cnt != ++cnt){
+               cnt = packet.packed_cnt;
+               PIN(error) = 1;
+            }else{
+               PIN(error) = 0;
+            }
+            PIN(pos_cmd) = packet.drive[0].pos;
+         }
+         
+      }
+            
+      rxpos += available;
+      rxpos %= sizeof(rxbuf);
+      PIN(rxfoo) = available;
+   }
+);
+
+FRT(
+
+);
+
+
+NRT(
+
+);
+
+ENDCOMP;
@@ -114,6 +114,7 @@ RT(
   float velmin = MAX(-PIN(max_vel) * p, -PIN(max_usr_vel));//MAX(MAX(velfb + accmin * period, -PIN(max_vel) * p) , -PIN(max_usr_vel));
   float velmax = MIN(PIN(max_vel) * p, PIN(max_usr_vel));//MIN(MIN(velfb + accmax * period, PIN(max_vel) * p), PIN(max_usr_vel));
   float velerr;
+   float velerr_p;
   float velsat;

   float posextcmd = PIN(pos_ext_cmd);
@@ -157,10 +158,11 @@ RT(

      // vel -> acc
      velerr = velcmd - velfb;
+      velerr_p = velerr;
      if(ABS(velerr) < PIN(vel_min)){
-         velerr = 0;
+         velerr_p = 0;
      }
-      acccmd = velerr / period * velp + PIN(acc_ff) * accextcmd; // acc = p * vel_error / period + ff2
+      acccmd = velerr_p / period * velp + PIN(acc_ff) * accextcmd; // acc = p * vel_error / period + ff2
      acccmd = acccmd * vellp + (1.0 - vellp) * old_acccmd; // lowpass
      accsat = SAT2(acccmd, accmin * accp, accmax * accp);
      acccmd = CLAMP(acccmd, accmin * accp, accmax * accp); // min/max clamping