126 lines
2.4 KiB
C
126 lines
2.4 KiB
C
#include <msp430.h>
|
|
#include "SPI.h"
|
|
#include "motorDriver.h"
|
|
//#include "motionController.h"
|
|
|
|
/* pinout
|
|
*
|
|
* motor driver:
|
|
* CLK <-> P1.4
|
|
* MOSI <-> P1.1
|
|
* MISO <-> P1.2
|
|
* CS <-> P1.3
|
|
*
|
|
* motoren:
|
|
* stapR <-> P1.6
|
|
* dirR <-> P2.5
|
|
* stapL <-> P1.7
|
|
* dirL <-> P2.6
|
|
*
|
|
* Lijndetectie:
|
|
* front right <-> P2.0
|
|
* front left <-> P2.1
|
|
* back right <-> P2.2
|
|
* back left <-> P2.3
|
|
*
|
|
* */
|
|
|
|
/*
|
|
* 0: motor test
|
|
* 1: NFC test
|
|
* 2: motioncontroller test
|
|
*/
|
|
#define TEST 0
|
|
|
|
#define STEP_PINS BIT4 | BIT5 // PORT 2
|
|
#define DIRr_PIN BIT6 // PORT 1
|
|
#define DIRl_PIN BIT7 // PORT 1
|
|
enum Direction {Forward=0, Backward=(DIRl_PIN | DIRr_PIN), RightTurn=DIRr_PIN, LeftTurn=DIRl_PIN};
|
|
|
|
const uchar v = 36;
|
|
const ulong halfRound = 2800;
|
|
|
|
void stap(uint n, ulong t){
|
|
ulong d;
|
|
uint s = n-100;
|
|
while(n != 0){
|
|
n--;
|
|
P2OUT |= STEP_PINS;
|
|
P2OUT &= ~STEP_PINS;
|
|
if(s < n || n < 100)
|
|
d = 100; // a crude way of a accaleration
|
|
else
|
|
d = t;
|
|
while(d != 0){
|
|
d--;
|
|
__delay_cycles(160);
|
|
}
|
|
}
|
|
__delay_cycles(160000);
|
|
}
|
|
|
|
void dir(enum Direction d){
|
|
P1OUT &= ~(DIRr_PIN | DIRl_PIN); // set dir bits to 0
|
|
P1OUT |= d;
|
|
}
|
|
|
|
|
|
/**
|
|
* main.c
|
|
*/
|
|
|
|
int main(void)
|
|
{
|
|
int i, ii;
|
|
WDTCTL = WDTPW | WDTHOLD; // stop watchdog timer
|
|
|
|
// set cpu op 16MHz
|
|
BCSCTL1 = CALBC1_16MHZ;
|
|
DCOCTL = CALDCO_16MHZ;
|
|
|
|
P2DIR |= STEP_PINS;
|
|
P1DIR |= DIRr_PIN | DIRl_PIN;
|
|
|
|
SPIInit();
|
|
MDInit();
|
|
//MCInit();
|
|
//NFCInit();
|
|
|
|
__delay_cycles(16000000);
|
|
|
|
for(ii=2; ii>0; ii--){
|
|
dir(Forward);
|
|
stap(10000, v);
|
|
dir(RightTurn);
|
|
stap(halfRound, v*2);
|
|
dir(Forward);
|
|
stap(10000, v);
|
|
dir(LeftTurn);
|
|
stap(halfRound, v*2);
|
|
}
|
|
for(ii=2; ii>0; ii--){
|
|
for(i=5; i>0; i--){
|
|
dir(Forward);
|
|
stap(1600, v);
|
|
dir(RightTurn);
|
|
stap(halfRound/2, v*2);
|
|
}
|
|
dir(Forward);
|
|
stap(1600, v);
|
|
dir(RightTurn);
|
|
stap(halfRound, v*2);
|
|
for(i=5; i>0; i--){
|
|
dir(Forward);
|
|
stap(1600, v);
|
|
dir(LeftTurn);
|
|
stap(halfRound/2, v*2);
|
|
}
|
|
dir(Forward);
|
|
stap(1600, v);
|
|
dir(LeftTurn);
|
|
stap(halfRound, v*2);
|
|
}
|
|
|
|
return 0;
|
|
}
|