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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
|
/*
* Thread_Winder.c
*
* Created on: 25 ���� 2018
* Author: shlomo
*/
#include"include.h"
#include "thread.h"
#include "Drivers/Peripheral_GPIO/GPIO.h"
#include "drivers/Motors/Motor.h"
#include "StateMachines/Printing/PrintingSTM.h"
#include "Modules/Control/Control.h"
#include "drivers/FPGA/FPGA.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
bool Winder_ScrewHoming = false;
uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag);
uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue);
bool ScrewCurrentDirection = false; //holds current screw direction
bool BusyfirstCall = true; //Ignores first call after activating the screw - too early
double ScrewSpeed = 0; //ScrewSpeed
uint32_t ScrewControlId = 0xFF;
uint32_t ScrewNumberOfSteps = 0; //holds the current number of steps for the next screw run - will be used to build the cone
uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw - will be used to build the cone
uint32_t Winder_Init(void)
{
return OK;
}
/*
*uint32_t Winder_Prepare(void *JobDetails)
* 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
* report ready to the job STM
*/
uint32_t Winder_Prepare(void)
{
uint32_t status = 0;
//JobTicket* JobTicket = JobDetails;
//float process_speed = JobTicket->processparameters->dyeingspeed;
double ScrewSpeed = 500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); // we will use pulley radius of the screw for this purpose, as of now
/*
* 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
* 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
* report ready to the job STM
*/
if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
{
Winder_PrepareStage2(0,0);
}
else
{
Winder_ScrewHoming = true;
status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2);
}
return status;
}
/*
* uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
* 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done
* report ready to the job STM
*/
uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
{
uint32_t status=OK;
uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency);
status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_SCREW, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize),numOfSteps, Winder_ScrewAtOffsetCallback);
//set motor location 0 here
return status;
}
uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
{
SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home
ScrewCurrentDirection = false;
BusyfirstCall = true;
ScrewSpeed = 0;
ScrewControlId = 0xFF;
ScrewNumberOfSteps = 0;
DirectionChangeCounter = 0;
PrepareReady(Module_Winder, ModuleDone);
return OK;
}
/*
* this is the main operational function of the screw - run back and forth until stopped
*/
/*
bool ScrewCurrentDirection = false;
bool BusyfirstCall = true;
double ScrewSpeed = 0;
uint32_t ScrewControlId = 0xFF;
uint32_t ScrewNumberOfSteps = 0;
uint32_t DirectionChangeCounter = 0;
*/
uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
{
if (BusyfirstCall)
{
BusyfirstCall = false;
return OK;
}
//ScrewCurrentDirection: false moves out, true moves home
if (BusyFlag == NOTBUSY)
{
if (ScrewCurrentDirection == false)
{
ScrewCurrentDirection = true;
}
else
{
ScrewCurrentDirection = false;
}
MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
DirectionChangeCounter++;
}
/*
* calculate new ScrewSpeed and call MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
*
*/
/*
* calculate cone shape according to DirectionChangeCounter and update ScrewNumberOfSteps
*/
return OK;
}
uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
{
return PreSegmentReady(Module_Winder,ModuleDone);
}
uint32_t Winder_Presegment(void *JobDetails)
{
JobTicket* JobTicket = JobDetails;
int process_speed = JobTicket->processparameters->dyeingspeed;
float screw_speed = 0;
float RotationsPerSecond;
ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
// * speed is set by the winding parameters and by winder rotational speed (read POSITION every 10msec)
// * calculate
// * 1. calculate speed according to JobTicket->processparameters->dyeingspeed
// * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
screw_speed = InternalWinderCfg.segmentoffsetpulses / InternalWinderCfg.NumberOfRotationPerPassage;
// calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI);
// calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
ScrewSpeed = screw_speed*RotationsPerSecond;
MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
//screw_speed = InternalWinderCfg.milimetersperrotation
// * 2. determine optimal micro-step setting
// * 3. calculate cart travel length from winding parameters
// * 4. start move of travel length
// * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
// MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_speed,WinderPresegmentReady);
//in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
PreSegmentReady(Module_Winder,ModuleDone);
return OK;
}
uint32_t Winder_End(void)
{
int pend;
//stop screw
ScrewNumberOfSteps = 0;
if (ScrewControlId != 0xFF)
RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
pend = MillisecFlushMsgQ(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
}
void Winder_ScrewHomeLimitSwitchInterrupt(void)
{
uint32_t status;
if (Winder_ScrewHoming)
{
MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
}
status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
}
void Winder_ScrewOutLimitSwitchInterrupt(void)
{
uint32_t status;
status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
}
|
