187 lines
4.0 KiB
C
187 lines
4.0 KiB
C
/*
|
|
* motionController.c
|
|
*
|
|
* TMC429
|
|
*
|
|
* Created on: 5 mei 2020
|
|
* Author: mreenen
|
|
*/
|
|
|
|
#include <msp430.h>
|
|
#include "typedefExtention.h"
|
|
#include "SPI.h"
|
|
#include "motionController.h"
|
|
|
|
const uchar MC_CS = BIT0;
|
|
|
|
// target position 24RW
|
|
#define MC_XTar1 0x00
|
|
#define MC_XTar2 0x20
|
|
#define MC_XTar3 0x40
|
|
// actual position 24R (RW if in velocity or hold mode)
|
|
#define MC_XAtu1 0x02
|
|
#define MC_XAtu2 0x22
|
|
#define MC_XAtu3 0x42
|
|
// velocity minimum (can be stopt emidetly) 11uRW
|
|
#define MC_VMin1 0x04
|
|
#define MC_VMin2 0x24
|
|
#define MC_VMin3 0x44
|
|
// velocity maximum 11uRW
|
|
#define MC_VMax1 0x06
|
|
#define MC_VMax2 0x26
|
|
#define MC_VMax3 0x46
|
|
// velocity target 12sRW
|
|
#define MC_VTar1 0x08
|
|
#define MC_VTar2 0x28
|
|
#define MC_VTar3 0x48
|
|
// velocity actual 12sR
|
|
#define MC_VAtu1 0x0A
|
|
#define MC_VAtu2 0x2A
|
|
#define MC_VAtu3 0x4A
|
|
// acceleration maximum 11uRW
|
|
#define MC_AMax1 0x0C
|
|
#define MC_AMax2 0x2C
|
|
#define MC_AMax3 0x4C
|
|
// acceleration actual 12sRW
|
|
#define MC_AAtu1 0x0E
|
|
#define MC_AAtu2 0x2E
|
|
#define MC_AAtu3 0x4E
|
|
// acceleration threshold 11uRW
|
|
#define MC_AAtu1 0x0E
|
|
#define MC_AAtu2 0x2E
|
|
#define MC_AAtu3 0x4E
|
|
// P value for ramp mode
|
|
#define MC_P1 0x12
|
|
#define MC_P2 0x32
|
|
#define MC_P3 0x52
|
|
// ramp_mode, ref conf and lp
|
|
#define MC_Rmcl1 0x14
|
|
#define MC_Rmcl2 0x34
|
|
#define MC_Rmcl3 0x54
|
|
#define MC_lp 0x010000
|
|
#define MC_REF_CONF 8
|
|
#define MC_RAMP_MODE 0b00
|
|
#define MC_SOFT_MODE 0b01
|
|
#define MC_VELOCITY_MODE 0b10
|
|
#define MC_HOLD_MODE 0b11
|
|
// deviders
|
|
#define MD_Div1 0x18
|
|
#define MD_Div2 0x38
|
|
#define MD_Div3 0x58
|
|
#define MD_PULSE_DIV 12
|
|
#define MD_RAMP_DIV 8
|
|
// IF_CONFIGURATION_429
|
|
#define MD_IF_CONF_429 0x68
|
|
#define MD_InvRef 0x001
|
|
#define MD_SdoInt 0x002
|
|
#define MD_StepHalf 0x004
|
|
#define MD_InvStp 0x008
|
|
#define MD_InvDir 0x010
|
|
#define MD_EnSd 0x020
|
|
#define MD_PosCompSel 6
|
|
#define MD_EnRefr 0x100
|
|
|
|
|
|
//TODO: make function to calculate and write P
|
|
|
|
void MC_write(uchar addr, ulong data) {
|
|
char i;
|
|
|
|
// wait for SPI bus to be available
|
|
SPIInitSend(MC_CS, SPI_Mode3, SPI_Clock_1MHz);
|
|
|
|
// set write bit to write
|
|
addr &= ~0x01;
|
|
SPI_packet[1] = addr;
|
|
|
|
// save data to SPI packet buffer
|
|
for(i=4; i>1; i--){
|
|
SPI_packet[i] = data;
|
|
data >>= 8;
|
|
}
|
|
|
|
SPI_packetC = 1; // set counter to one so it skips byte 0 and sends 4 bytes
|
|
SPISend();
|
|
}
|
|
|
|
ulong MC_read(unsigned char addr) {
|
|
char i;
|
|
ulong data;
|
|
|
|
// wait for SPI bus to be available
|
|
SPIInitSend(MC_CS, SPI_Mode3, SPI_Clock_1MHz);
|
|
|
|
// set write bit to read
|
|
addr |= 0x01;
|
|
SPI_packet[1] = addr;
|
|
|
|
SPI_packetC = 1; // set counter to one so it skips byte 0 and sends 4 bytes
|
|
SPISend();
|
|
|
|
// wait for SPI bus to be available
|
|
SPIInitSend(MC_CS, SPI_Mode3, SPI_Clock_1MHz);
|
|
|
|
// set write bit to read
|
|
addr |= 0x01;
|
|
SPI_packet[1] = addr;
|
|
|
|
SPI_packetC = 1; // set counter to one so it skips byte 0 and sends 4 bytes
|
|
SPISend();
|
|
|
|
// read data
|
|
for(i=2; i<5; i++){
|
|
data |= (ulong) SPI_packet[i];
|
|
data <<= 8;
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
void MCInit(){
|
|
ulong c;
|
|
|
|
P1DIR |= MC_CS; // set MC_CS (pin 5) as output
|
|
P1OUT |= MC_CS; // set MC_CS high
|
|
|
|
|
|
// set min and max velocity
|
|
MC_write(MC_VMin1, 0);
|
|
MC_write(MC_VMin2, 0);
|
|
MC_write(MC_VMax1, 2047);
|
|
MC_write(MC_VMax2, 2047);
|
|
|
|
// set clock dividers
|
|
MC_write(MD_Div1, (9 << MD_PULSE_DIV) | (3 << MD_RAMP_DIV));
|
|
MC_write(MD_Div2, (9 << MD_PULSE_DIV) | (3 << MD_RAMP_DIV));
|
|
|
|
// max acceleration
|
|
MC_write(MC_AMax1, 1);
|
|
MC_write(MC_AMax2, 1);
|
|
|
|
// set ramp mode and reference switch configuration
|
|
MC_write(MC_Rmcl1, (0b0111 << MC_REF_CONF) | MC_VELOCITY_MODE);
|
|
MC_write(MC_Rmcl2, (0b0111 << MC_REF_CONF) | MC_VELOCITY_MODE);
|
|
|
|
// set to stap/dir mode
|
|
c = MC_read(MD_IF_CONF_429);
|
|
MC_write(MD_IF_CONF_429, c | MD_EnSd);
|
|
|
|
//DEBUG
|
|
MC_read(MC_VMax1);
|
|
MC_read(MC_Rmcl2);
|
|
MC_read(MC_AMax1);
|
|
}
|
|
|
|
void MCSetV(enum MC_motor m, int v){
|
|
uchar addr = MC_VTar1 | m;
|
|
|
|
// shorten the int
|
|
if(v < 0)
|
|
v |= 0x0800; // set signed bit
|
|
else
|
|
v &= ~0x0800; // remove signed bit
|
|
v &= 0x0FFF; // contraint to 12 bits
|
|
|
|
MC_write(addr, v);
|
|
}
|