예제 #1
0
        public void StartPVT(Motors motors, string adress,double unit,int timevalue1,int timevalue2)
        {
            #region
            //计算轨迹位置,速度和时间间隔序列
            //原始数据

            string[] ral = File.ReadAllLines(adress, Encoding.Default); //相对目录是在bin/Debug下,所以要回溯到上两级目录
            int lineCounter = ral.Length; //获取步态数据行数
            string[] col = (ral[0] ?? string.Empty).Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
            int colCounter = col.Length; //获取步态数据列数
            double[,] pos0 = new double[lineCounter, colCounter]; //原始位置数据
            double[,] vel = new double[lineCounter, colCounter]; //速度
            int[] times = new int[lineCounter]; //时间间隔
            for (int i = 0; i < lineCounter; i++)
            {
                if(i<lineCounter-300)
                {
                    times[i] = timevalue1; //【设置】时间间隔
                }
                else
                {
                    times[i] = timevalue2; //【设置】时间间隔
                }

                string[] str = (ral[i] ?? string.Empty).Split(new char[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
                for (int j = 0; j < colCounter; j++)
                {
                    pos0[i, j] = double.Parse(str[j]) / (unit / motors.RATIO) * motors.userUnits[j] * -1;
                }
            }
            times[lineCounter - 1] = 0;
            #region
            //一次扩充
            //int richLine = lineCounter * 2 - 1;
            //double[,] pos1 = new double[richLine, colCounter]; //一次扩充位置数据
            //for (int i = 0; i < richLine; i++)
            //{
            //    for (int j = 0; j < colCounter; j++)
            //    {
            //        if (i % 2 == 0)//偶数位存放原始数据
            //        {
            //            pos1[i, j] = pos0[i / 2, j];
            //        }
            //        else//奇数位存放扩充数据
            //        {
            //            pos1[i, j] = (pos0[i / 2 + 1, j] + pos0[i / 2, j]) / 2.0;
            //        }
            //    }
            //}

            ////二次扩充
            //int rich2Line = richLine * 2 - 1;
            //double[,] pos2 = new double[rich2Line, colCounter]; //二次扩充位置数据
            //double[,] vel = new double[rich2Line, colCounter]; //速度
            //int[] times = new int[rich2Line]; //时间间隔
            //for (int i = 0; i < rich2Line; i++)
            //{
            //    times[i] =3; //【设置】时间间隔
            //    for (int j = 0; j < colCounter; j++)
            //    {

            //        if (i % 2 == 0)//偶数位存放原始数据
            //        {
            //            pos2[i, j] = pos1[i / 2, j];
            //        }
            //        else//奇数位存放扩充数据
            //        {
            //            pos2[i, j] = (pos1[i / 2 + 1, j] + pos1[i / 2, j]) / 2.0;
            //        }
            //    }
            //}

            //三次扩充
            //int rich3Line = rich2Line * 2 - 1;

            //double[,] pos3 = new double[rich2Line, colCounter]; //三次扩充位置数据

            //for (int i = 0; i < rich3Line; i++)
            //{

            //    for (int j = 0; j < colCounter; j++)
            //    {
            //        if (i % 2 == 0)//偶数位存放原始数据
            //        {
            //            pos3[i, j] = pos2[i / 2, j];
            //        }
            //        else//奇数位存放扩充数据
            //        {
            //            pos3[i, j] = (pos2[i / 2 + 1, j] + pos2[i / 2, j]) / 2.0;

            //        }
            //        //pos3[i, j] = pos3[i, j] - pos3[0, j];
            //    }
            //}
            #endregion

            for (int i = 0; i < lineCounter - 1; i++)
            {
                for (int j = 0; j < colCounter; j++)
                {

                    vel[i, j] = (pos0[i + 1, j] - pos0[i, j]) * 1000.0 / (times[i]);
                }
            }
            vel[lineCounter - 1, 0] = 0;
            vel[lineCounter - 1, 1] = 0;
            vel[lineCounter - 1, 2] = 0;
            vel[lineCounter - 1, 3] = 0;
            #endregion

            for (int i = 0; i < motors.motor_num; i++)//开始步态前各电机回到轨迹初始位置
            {
                motors.profileSettingsObj = motors.ampObj[i].ProfileSettings;
                motors.profileSettingsObj.ProfileAccel = (motors.ampObj[i].VelocityLoopSettings.VelLoopMaxAcc);
                motors.profileSettingsObj.ProfileDecel = (motors.ampObj[i].VelocityLoopSettings.VelLoopMaxDec);
                motors.profileSettingsObj.ProfileVel = (motors.ampObj[i].VelocityLoopSettings.VelLoopMaxVel * 0.8);
                motors.profileSettingsObj.ProfileType = CML_PROFILE_TYPE.PROFILE_TRAP; //PVT模式下的控制模式类型
                motors.ampObj[i].ProfileSettings = motors.profileSettingsObj;
                motors.ampObj[i].MoveAbs(pos0[0, i]); //PVT模式开始后先移动到各关节初始位置
                motors.ampObj[i].WaitMoveDone(10000); //等待各关节回到初始位置的最大时间
            }

            motors.Linkage.TrajectoryInitialize(pos0, vel, times, 500); //开始步态
            //motors.ampObj[0].WaitMoveDone(10000);
            //motors.ampObj[1].WaitMoveDone(10000);
            //motors.ampObj[2].WaitMoveDone(10000);
            //motors.ampObj[3].WaitMoveDone(10000);
        }
예제 #2
0
        private const int PARA_NUM = 1; //每个电机写入参数个数

        #endregion

        public bool writeStart(StatusBar statusBarIn, TextBlock statusInfoTextBlockIn, Motors motorsIn)
        {
            statusBar           = statusBarIn;
            statusInfoTextBlock = statusInfoTextBlockIn;
            motors = motorsIn;

            ExcelApp = new Excel.Application();
            if (ExcelApp == null)
            {
                statusBar.Background     = new SolidColorBrush(Color.FromArgb(255, 230, 20, 20));
                statusInfoTextBlock.Text = "该计算机可能未安装Excel!";
                return(false);
            }

            ExcelApp.Visible = true;
            ExcelApp.Application.DisplayAlerts = false;
            ExcelWorkbooks = ExcelApp.Workbooks;
            try
            {
                ExcelWorkbook  = ExcelWorkbooks.Add(misValue);
                ExcelWorksheet = (Excel.Worksheet)ExcelWorkbook.Worksheets["Sheet1"];
                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 1], ExcelWorksheet.Cells[1, PARA_NUM]].Merge();
                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 1], ExcelWorksheet.Cells[1, PARA_NUM]].Interior.ColorIndex = 39;
                ExcelWorksheet.Cells[1, 1] = "电机1左膝";
                ExcelWorksheet.Cells[2, 1] = "关节角度";

                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, PARA_NUM + 1], ExcelWorksheet.Cells[1, 2 * PARA_NUM]].Merge();
                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, PARA_NUM + 1], ExcelWorksheet.Cells[1, 2 * PARA_NUM]].Interior.ColorIndex = 35;
                ExcelWorksheet.Cells[1, PARA_NUM + 1] = "电机2左髋";
                ExcelWorksheet.Cells[2, PARA_NUM + 1] = "关节角度";

                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 2 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 3 * PARA_NUM]].Merge();
                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 2 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 3 * PARA_NUM]].Interior.ColorIndex = 37;
                ExcelWorksheet.Cells[1, 2 * PARA_NUM + 1] = "电机3右髋";
                ExcelWorksheet.Cells[2, 2 * PARA_NUM + 1] = "关节角度";

                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 3 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 4 * PARA_NUM]].Merge();
                ExcelWorksheet.Range[ExcelWorksheet.Cells[1, 3 * PARA_NUM + 1], ExcelWorksheet.Cells[1, 4 * PARA_NUM]].Interior.ColorIndex = 36;
                ExcelWorksheet.Cells[1, 3 * PARA_NUM + 1] = "电机4右膝";
                ExcelWorksheet.Cells[2, 3 * PARA_NUM + 1] = "关节角度";

                excelCnt = 3;

                timer          = new DispatcherTimer();
                timer.Interval = TimeSpan.FromMilliseconds(10);
                timer.Tick    += new EventHandler(writeExcel_Tick);
                timer.Start();
            }
            catch
            {
                statusBar.Background     = new SolidColorBrush(Color.FromArgb(255, 230, 20, 20));
                statusInfoTextBlock.Text = "打开文件时发生错误!";
                writeStop();
                return(false);
            }
            return(true);
        }
예제 #3
0
        // 求取从初始位置到第一个位置的电机转角变化量的绝对值

        public void start_Standup2(Motors motor)
        {
            double[] DeltaP1 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP1[i] = System.Math.Abs(KeyPos1[i] - 0);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP1 = DeltaP1.Max();

            //设置各电机运动参数,并在位置模式下运动到第一个下蹲位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP1[i] / MaxDeltaP1; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos1[i]);
            }


            //cm.MotorAmp_Array[9].WaitMoveDone(100000);
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP2 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP2[i] = System.Math.Abs(KeyPos2[i] - KeyPos1[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP2 = DeltaP2.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP2[i] / MaxDeltaP2; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos2[i]);
            }
            //cm.MotorAmp_Array[9].WaitMoveDone(100000);

            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP3 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP3[i] = System.Math.Abs(KeyPos3[i] - KeyPos2[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP3 = DeltaP3.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚落地位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP3[i] / MaxDeltaP3; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos3[i]);
            }
            //cm.MotorAmp_Array[9].WaitMoveDone(100000);
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP4 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP4[i] = System.Math.Abs(KeyPos4[i] - KeyPos3[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP4 = DeltaP4.Max();

            //设置各电机运动参数,并在位置模式下左移重心
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP4[i] / MaxDeltaP4; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos4[i]);
            }
            //MotorAmp_Array[9].WaitMoveDone(100000);
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP5 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP5[i] = System.Math.Abs(KeyPos5[i] - KeyPos4[i]);
            }
            //获取变化量的绝对值的最大值
            double MaxDeltaP5 = DeltaP5.Max();

            //设置各电机运动参数,并在位置模式下左移重心
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = 0 * 6400 * 4 * 160 / 360;                 //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360; //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360; //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos5[i]);
            }
            Task task = Task.Run(() => {
                Thread.Sleep(200);
            });

            task.Wait();
            //Thread.Sleep(2000);
            //motor.ampObj[3].WaitMoveDone(5000);
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP6 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP6[i] = System.Math.Abs(KeyPos6[i] - KeyPos5[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP6 = DeltaP6.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP6[i] / MaxDeltaP6; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos6[i]);
            }
            //cm.MotorAmp_Array[9].WaitMoveDone(100000);
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP7 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP7[i] = System.Math.Abs(KeyPos7[i] - KeyPos6[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP7 = DeltaP7.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP7[i] / MaxDeltaP7; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos7[i]);
            }
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP8 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP8[i] = System.Math.Abs(KeyPos8[i] - KeyPos7[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP8 = DeltaP8.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP8[i] / MaxDeltaP8; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos8[i]);
            }
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP9 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP9[i] = System.Math.Abs(KeyPos9[i] - KeyPos8[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP9 = DeltaP9.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP9[i] / MaxDeltaP9; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                       //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                       //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos9[i]);
            }
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP10 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP10[i] = System.Math.Abs(KeyPos10[i] - KeyPos9[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP10 = DeltaP10.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP10[i] / MaxDeltaP10; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                         //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                         //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos10[i]);
            }
            //求取相对前一位置的电机转角变化量的绝对值
            double[] DeltaP11 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP11[i] = System.Math.Abs(KeyPos11[i] - KeyPos10[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP11 = DeltaP11.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP11[i] / MaxDeltaP11; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                         //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                         //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos11[i]);
            }
            double[] DeltaP12 = new double[4];
            for (int i = 0; i < motor.motor_num; i++)
            {
                DeltaP12[i] = System.Math.Abs(KeyPos12[i] - KeyPos11[i]);
            }

            //获取变化量的绝对值的最大值
            double MaxDeltaP12 = DeltaP12.Max();

            //设置各电机运动参数,并在位置模式下运动到第左脚抬升位置
            for (int i = 0; i < motor.motor_num; i++)
            {
                profileParameters               = motor.ampObj[i].ProfileSettings;
                profileParameters.ProfileVel    = MotorVelocity * 6400 * 4 * 160 / 360 * DeltaP12[i] / MaxDeltaP12; //单位为°/s
                profileParameters.ProfileAccel  = MotorAcceleration * 6400 * 4 * 160 / 360;                         //单位为°/s2
                profileParameters.ProfileDecel  = MotorDeceleration * 6400 * 4 * 160 / 360;                         //单位为°/s
                profileParameters.ProfileType   = CML_PROFILE_TYPE.PROFILE_TRAP;
                motor.ampObj[i].ProfileSettings = profileParameters;
                motor.ampObj[i].MoveAbs(KeyPos12[i]);
            }
        }