Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c


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
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

namespace Tango.MachineStudio.UI.Views
{
    /// <summary>
    /// Interaction logic for FirmwareUpgradeView.xaml
    /// </summary>
    public partial class FirmwareUpgradeView : UserControl
    {
        public FirmwareUpgradeView()
        {
            InitializeComponent();
        }
    }
}
/*
 * Thread_Winder.c
 *
 *  Created on: 25 áîøõ 2018
 *      Author: shlomo
 */
#include"include.h"
#include "thread.h"

#include "drivers/Motors/Motor.h"
#include "StateMachines/Printing/PrintingSTM.h"
#include "Modules/Control/Control.h"
#include "Modules/Control/MillisecTask.h"

#include "modules/General/process.h"

#include "Common/report/report.h"

#include "drivers/FPGA/FPGA.h"

#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"

#include <driverlib/timer.h>
#include <inc/hw_ints.h>
#include <inc/hw_memmap.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;
double ScrewRunningTime = 0;
bool ScrewDirection = false;
bool SCREW_TimerActivated = false;
//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

InternalWinderConfigStruc InternalWinderCfg = {0};
void ScrewTimerInterruptInit(void);
void ScrewsStopControlTimer (void);
void ScrewsStartControlTimer (void);

uint32_t Winder_Init(void)
{
    ScrewTimerInterruptInit();
    return OK;
}


uint32_t InternalWinderConfigMessage(HardwareWinder* request)
{
    uint32_t status = PASSED;

    InternalWinderCfg.milimetersperrotation = request->millimeterperrotation;

    return status;
}
uint32_t InternalWindingConfigMessage(JobSpool* request)
{
    uint32_t status = PASSED;

    InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
    InternalWinderCfg.spoolbackingrate = request->backingrate;
    InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
    InternalWinderCfg.diameter = request->diameter;

    return status;
}
/*
 *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 = 1500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
    //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
    /*
     * 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)
    {
        //REPORT_MSG(LIMIT, "Winder_Prepare at limit");
        Winder_PrepareStage2(0,0);
    }
    else
    {
        //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
        Winder_ScrewHoming = true;
        //REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, "Winder_Prepare move to limit");
        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,InternalWinderCfg.segmentoffsetpulses);
    //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
    REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency, "Winder_PrepareStage2");

    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
    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);

    ScrewCurrentDirection = false;
    ScrewSpeed = 0;
    ScrewControlId = 0xFF;
    ScrewNumberOfSteps = 0;
    DirectionChangeCounter = 0;
    REPORT_MSG(BusyFlag, "Winder_ScrewAtOffsetCallback");
    PrepareReady(Module_Winder, ModuleDone);
    return OK;
}

/*
 * this is the main operational function of the screw - run back and forth until stopped
 */
/*
    InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
    InternalWinderCfg.spoolbackingrate = request->backingrate;
    InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
    InternalWinderCfg.SpoolBottomBackingRate =  request->bottombackingrate;
    InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage;
 *
Calculate the number of steps.
Initial home position = ...
Initial out movement = ScrewNumberOfSteps
if DirectionChangeCounter %= backingrate : reduce one from the  ScrewNumberOfSteps
if DirectionChangeCounter %= bottombackingrate && direction was out: ADD one to the  ScrewNumberOfSteps,
// WRONG? if the flag is raised - lower it and reduce one from the  ScrewNumberOfSteps
30:100 - 70
30:99 - 69
30:98 - 68
29:98 - 69
29:97 -68
InternalWinderCfg.segmentoffsetpulses
  int32_t backingrate;
  int32_t bottombackingrate;
  
numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
*/
uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
{
    uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
    float screw_horizontal_speed = 0;
    float RotationsPerSecond;

        if (Counter%InternalWinderCfg.spoolbackingrate == 0)
        {
            NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
            ScrewNumberOfSteps--;
            screw_horizontal_speed = ScrewNumberOfSteps /  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 =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
            // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
            ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;

            //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
            //REPORT_MSG(ScrewSpeed, "CalculateNumberOfSteps ScrewSpeed");
            //REPORT_MSG(CurrentControlledSpeed[WINDER_MOTOR], "CalculateNumberOfSteps Winder speed");
            CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;

        }
        if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1))
        {
            if (direction == MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize)//because the direction is already reversed at the interrupt
            {
                NumberOfSteps += (Counter/InternalWinderCfg.SpoolBottomBackingRate);
                ScrewNumberOfSteps++;
                REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
            }

        }
    return NumberOfSteps;

}
uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
{
    uint32_t Steps;
    double temp;
    if (BusyfirstCall)
    {
        BusyfirstCall = false;
		DirectionChangeCounter++;
        return OK;
    }
    //ScrewCurrentDirection: false moves out, true moves home
    if (BusyFlag == NOTBUSY)
    {
        CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection);
        Steps = ScrewNumberOfSteps;
/*        if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
        {
            ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
            Steps += InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
            REPORT_MSG(LIMIT, "Winder at  right limit");
        }
        else if (FPGA_Read_limit_Switches(GPI_LS_SCREW_LEFT)==LIMIT)
        {
            ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
            REPORT_MSG(LIMIT, "Winder at left limit");
        }
*/
        temp = SYS_CLK_FREQ;
        temp *= Steps;
        temp /= ScrewSpeed;
        //LOG_ERROR(Steps,"ScrewDirectionChange - ScrewNumberOfSteps");

        /*if (ScrewRunningTime != temp)
        {
            REPORT_MSG(temp, "ScrewDirectionChange ScrewRunningTime changed");
        }*/
        ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
        //MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back.
    }
    return OK;

}

uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue)
{
    return PreSegmentReady(Module_Winder,ModuleDone);
}

uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
{
    //JobTicket* JobTicket = JobDetails;
    float screw_horizontal_speed = 0;
    float RotationsPerSecond;
    char Message[80];
	double temp = 0;
    if (dyeingspeed == 0)
    {
        LOG_ERROR (dyeingspeed," job speed zero");
        return ERROR;
    }

    if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job)
    {
        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_horizontal_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 = dyeingspeed / (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulleyradius * PI);
        RotationsPerSecond =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
        // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
        ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;

        //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
        usnprintf(Message, 80, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
        //LOG_ERROR(segmentfirst_speed,Message);
        //Report(Message,__FILE__,__LINE__,Dispenser_i,RpWarning,segmentfirst_speed,0);
        SendJobProgress(0.0,0,false, Message);

        CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
        OriginalMotorSpd_2PPS[SCREW_MOTOR] = ScrewSpeed;
        //screw_horizontal_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;
        temp = SYS_CLK_FREQ;
        temp *= InternalWinderCfg.segmentoffsetpulses;
        temp /= ScrewSpeed;
        ScrewRunningTime = temp;//(SYS_CLK_FREQ*InternalWinderCfg.segmentoffsetpulses)/ScrewSpeed;
        LOG_ERROR(ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps");
        LOG_ERROR(ScrewRunningTime,"Winder pre segment - ScrewRunningTime");
//        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
        //ScrewDirection = 1-ScrewDirection;
        LOG_ERROR(ScrewSpeed,"Winder pre segment - ScrewSpeed");
        //MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
        ScrewsStartControlTimer ();
    }
    PreSegmentReady(Module_Winder,ModuleDone);

    return OK;
}
uint32_t Winder_End(void)
{
    int pend;
    //stop screw
    ScrewNumberOfSteps = 0;
    if (ScrewControlId != 0xFF)
        RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
    CurrentControlledSpeed[SCREW_MOTOR] = 0;
    ScrewsStopControlTimer();

    return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
}
void Winder_ScrewHomeLimitSwitchInterrupt(void)
{
    //uint32_t status;
    //handle glitch - send information to the next time that the motor stops
    if  (Winder_ScrewHoming)
    {
        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); //stop ASAP
    }
    MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
}
void Winder_ScrewOutLimitSwitchInterrupt(void)
{
    //handle glitch - send information to the next time that the motor stops
    //uint32_t status;
    MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
}

uint32_t  Screw_timerBase = TIMER3_BASE;        //Timer handle

bool     Screw_Restart = false;
void ScrewTimerInterruptInit(void)
{
    ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
    ROM_TimerEnable(Screw_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER3A);
    ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
    Screw_Restart = false;

}
void ScrewsStopControlTimer (void)
{
    SCREW_TimerActivated = false;
    ROM_TimerDisable(Screw_timerBase, TIMER_A);
    //ROM_IntDisable(INT_TIMER3A);
    //ROM_TimerIntDisable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
}
void ScrewsStartControlTimer (void)
{
    if (SCREW_TimerActivated == true)
        return;
    SCREW_TimerActivated = true;
    //ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
    TimerEnable(Screw_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER3A);
    ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
    ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)1200000/*10 millisec*/ );
    Report("ScrewsStartControlTimer direction,speed ", __FILE__,__LINE__,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0);
    return;
}
void ScrewTimerInterrupt(int ARG0)
{
    bool dir = ScrewCurrentDirection;
    ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    ROM_IntMasterDisable();
    ScrewDirectionChange(0,NOTBUSY);
    if (SCREW_TimerActivated == true)
    {
        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
        ScrewCurrentDirection = 1-ScrewCurrentDirection;
        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
        ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
    }
    else
    {
        TimerDisable(Screw_timerBase, TIMER_A);
    }
    //Report("ScrewTimerInterrupt duration, speed", __FILE__,__LINE__,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    return ;

}