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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
using System;
using System.Text;
using System.Linq;
using System.Drawing;
using System.Diagnostics;
using System.Windows.Forms;
using System.Threading;
using System.Threading.Tasks;
using System.Collections.Generic;
using Tango.PMR.Stubs;
using Tango.Stubs;
include "..\..\Defines\Tango_Defines_Basic.cs"
include "Tango_Defines_Motors.cs"
//include "C:\To_Backup\Mulberry\Mulberry_current\Customers\Twine\Tango\LAB_Tests\Main_PCB\ATP_Test\Scripts\Defines\Tango_Defines_Basic.cs"
//include "C:\To_Backup\Mulberry\Mulberry_current\Customers\Twine\Tango\LAB_Tests\Main_PCB\ATP_Test\Scripts\Defines\Tango_Defines_Motors.cs"
int Winder_Motor_CFG(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x05, 0x00400000); //Acceleration
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x06, 0x00400000); //Deceleration
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x07, 0x00ff0000); //Maximum speed
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x08, 0x00000000); //Minimum speed and low speed optimization
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x09, 0x01000000); //Holding Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0a, 0x20000000); //Constant speed Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0b, 0x20000000); //Acceleration starting Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0c, 0x20000000); //Deceleration starting Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0d, 0x22c50000); //BEMF comp. curve intersect speed
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0e, 0x07000000); //BEMF comp. curve starting slope
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0f, 0x15000000); //BEMF comp. curve final slope (acceleration)
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x10, 0x15000000); //BEMF comp. curve final slope (deceleration)
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x11, 0x00000000); //Thermal compensation factor
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x13, 0xf0000000); //OCD threshold - Needs to be optimized. For now set to maximum
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x14, 0x7f000000); //STALL threshold - Needs to be optimized. For now set to maximum
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x15, 0x00ff0000); //Full step speed
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x16, 0x04000000); //Step mode and sync signal setup: Step mode: 1/16 step, Sync signal: Disabled
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x17, 0xff000000); //Alarms enables
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x18, 0x1ca00000); //IC configuration: Int. osc @16MHz (no clock output),
//SW input: HardStop interrupt, Motor supply voltage compensation: Enabled,
//Overcurrent shutdown: Enabled, Output slew-rate: Fastest, fPWM = fosc x 2 / 512
return 1;
}
int Screw_Motor_CFG(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x05, 0x07ff0000); //Acceleration
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x06, 0x0fff0000); //Deceleration
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x07, 0x03ff0000); //Maximum speed - used for testing screw arm max speed
//Motor_SetParam( Fpga, HighAdr, LowAdr, 0x07, 0x000f0000); //Maximum speed - used for testing screw arm movent at low speed -> max. current
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x08, 0x00000000); //Minimum speed and low speed optimization
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x09, 0x01000000); //Holding Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0a, 0x20000000); //Constant speed Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0b, 0x20000000); //Acceleration starting Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0c, 0x20000000); //Deceleration starting Kval
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0d, 0x22c50000); //BEMF comp. curve intersect speed
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0e, 0x07000000); //BEMF comp. curve starting slope
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x0f, 0x15000000); //BEMF comp. curve final slope (acceleration)
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x10, 0x15000000); //BEMF comp. curve final slope (deceleration)
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x11, 0x00000000); //Thermal compensation factor
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x13, 0x0f000000); //OCD threshold
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x14, 0x7f000000); //STALL threshold - Needs to be optimized. For now set to maximum
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x15, 0x03ff0000); //Full step speed - Needs to be optimized. For now set to maximum
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x16, 0x05000000); //Step mode and sync signal setup: Step mode: 1/32 step, Sync signal: Disabled
// Motor_SetParam( Fpga, HighAdr, LowAdr, 0x17, 0xff000000); //Alarms enables. Commented out since register can only be written during "stop". Trying to write during "High-Z" caused busy-bit to be stuck low
Motor_SetParam( Fpga, HighAdr, LowAdr, 0x18, 0x1ca00000); //IC configuration: Int. osc @16MHz (no clock output),
//SW input: HardStop interrupt, Motor supply voltage compensation: Enabled,
//Overcurrent shutdown: Enabled, Output slew-rate: Fastest, fPWM = fosc x 2 / 512
return 1;
}
int Winder_Motor_Go(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Motor_Run(Fpga, HighAdr, LowAdr, 0x016f00, 0x1); // 7 rps, 1400 full steps/second
//Motor_Run(Fpga, HighAdr, LowAdr, 0x02de00, 0x1); //14 rps, 2800 full steps/second
//Motor_Run(Fpga, HighAdr, LowAdr, 0x0395, 0x1); //17.5 rps, 3500 full steps/second
//Motor_HighZ(Fpga, HighAdr, LowAdr);
return 1;
}
int Winder_Motor_Go_Ccr(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Motor_Run(Fpga, HighAdr, LowAdr, 0x016f00, 0x0); // 7 rps, 1400 full steps/second
//Motor_Run(Fpga, HighAdr, LowAdr, 0x02de00, 0x1); //14 rps, 2800 full steps/second
//Motor_Run(Fpga, HighAdr, LowAdr, 0x0395, 0x1); //17.5 rps, 3500 full steps/second
//Motor_HighZ(Fpga, HighAdr, LowAdr);
return 1;
}
int Screw_Motor_Go(Int32 Fpga, Int32 HighAdr, Int32 LowAdr)
{
Motor_Move_Loop_Screw(Fpga, HighAdr, LowAdr, 0x4200, 0x1, 4); //0x4200 x 2 1/32 steps, 100 times back and forth
Motor_HighZ(Fpga, HighAdr, LowAdr);
return 1;
}
public void OnExecute(StubManager stubManager)
{
Int32 Bit = 0 ;
Motor_OutOfReset();
Thread.Sleep(50);
Motor_DriverReset();
Thread.Sleep(50);
Motor_OutOfReset();
// Motor_HighZ(FPGA1, F1_MOTO_SCREW_A1_TX_01, F1_MOTO_SCREW_A1_TX_00);
Motor_HighZ(FPGA1, F1_MOTO_WINDER_A1_TX_01, F1_MOTO_WINDER_A1_TX_00);
//Configure motor drivers
Winder_Motor_CFG(FPGA1, F1_MOTO_WINDER_A1_TX_01, F1_MOTO_WINDER_A1_TX_00);
// Screw_Motor_CFG(FPGA1, F1_MOTO_SCREW_A1_TX_01, F1_MOTO_SCREW_A1_TX_00);
//Run/move motors
Winder_Motor_Go(FPGA1, F1_MOTO_WINDER_A1_TX_01, F1_MOTO_WINDER_A1_TX_00);
// Screw_Motor_Go(FPGA1, F1_MOTO_SCREW_A1_TX_01, F1_MOTO_SCREW_A1_TX_00);
Thread.Sleep(1000);
// Motor_HighZ(FPGA1, F1_MOTO_SCREW_A1_TX_01, F1_MOTO_SCREW_A1_TX_00);
Motor_HighZ(FPGA1, F1_MOTO_WINDER_A1_TX_01, F1_MOTO_WINDER_A1_TX_00);
Thread.Sleep(1000);
Winder_Motor_Go_Ccr(FPGA1, F1_MOTO_WINDER_A1_TX_01, F1_MOTO_WINDER_A1_TX_00);
Thread.Sleep(1000);
Motor_HighZ(FPGA1, F1_MOTO_WINDER_A1_TX_01, F1_MOTO_WINDER_A1_TX_00);
}
|
