1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103 | #include<18F4585.h>
#device ADC=10
#use delay(clock=20000000)
#use rs232(baud=19200,xmit=PIN_C6,rcv=PIN_C7, parity=N, bits=8, stop=2)
//#fuses HS,NOPUT,NOBROWNOUT,NOWRT,NOWDT,NOLVP
#fuses HS,NOPUT,BROWNOUT,NOWRT,NOWDT,NOLVP // from original omurs file--edit by jam
// HighSpeedcrystal, NOPowerUpTimer, NOBROWNOUTdetector,
// enableeepromWRiTe, NOWatchDogTimer, NOLowVoltageProgramming
#include "RB.c"
#include "hsr_serial.c"
#include "RCservo.c"
#int_rda
void serial_isr()
{
if(kbhit())
{
_RxData[_RxCounter]=getc();
_RxCounter++;
_RxCounter%=3;
if (_RxCounter==0)
{
_RxLastData[0]=_RxData[0];
_RxLastData[1]=_RxData[1];
_RxComplete=1;
}
}
}
// TIMER GUIDELINES
// Timer0 is used by RB Stack, leaving Timers 1,2 and 3 for Application.
// ResetApp is a callback function:
// it is called on startup from main(), and in addition,
// it can be called at any time by RB_ProcessStack();
// RB_App_SetFactoryDefaults is a callback, and is called only once,
// by RB_Initialize, on first power up after wiping the ROM.
void RB_App_SetFactoryDefaults()
{
uint16 tmp;
sint16 tmps;
tmps = 9001;//starts limp
RBSetEEPROM(&tmps,SERVO_START_POS,2);
/*tmp = 0;
RBSetEEPROM(&tmp,SERVO_KP, 2);
tmp = 0;
RBSetEEPROM(&tmp,SERVO_KD,2);
*/
tmp=255;
RBSetEEPROM(&tmp,SERVO_SPEED,2);
tmp=0;
RBSetEEPROM(&tmp,FEEDBACK,2);
tmps=0;
RBSetEEPROM(&tmp,SERVO_CAL_CMD_CENTER,2);
tmps=9000;
RBSetEEPROM(&tmps,SERVO_CAL_CMD_AMPLITUDE,2);
}
void RB_App_ResetApp()
{
//setup_spi(FALSE); // not using this peripheral
//setup_wdt(WDT_OFF); // no watchdog timer
disable_interrupts(GLOBAL); // to prevent data goin corrupt
RB_reset_from_eeprom();// reads node id from eeprom and resets variables associated with it
RB_Initialize();
RCservo_init(); // sets up servo default values
delay_ms(2000); // WARNING: DONOT REMOVE WILL CAUSE SERVO TO FREEZE
HSR_SerialInitialization(); // Initial Serial Configuration for HSR communication
delay_ms(2000); // WARNING: DONOT REMOVE WILL CAUSE SERVO TO FREEZE
output_toggle(Pin_D7);
}
// URBProcessStack uses all idle processor cycles.
// Application processing is done inside interrupts.
void ReadPos()
{ // feed_freq has to be less than 4C47 i.e. the URB_HEARTBEAT_INTERVAL
raw_adc = HSR_ReadPosition();
//actualPos=(sint16)(((float)( raw_adc - commandCenter))/ commandScaleFactor);
actualPos=( raw_adc*10 - 14500 );
RB_Send_PM(0);
output_toggle(PIN_D5);
}
void main()
{
output_low(Pin_D7);
RB_App_ResetApp();
while(1)
{
if(get_timer0() >= feed_int*feed_mult && feed_freq != 0)
{ _RxCounter=0;
ReadPos();
feed_mult++;
}
RB_ProcessStack();
initiate_pulse();
}
}
// end main
|