用action script 画圆 (use action draw a circle)

博客给出了使用函数绘制圆形的代码。定义了 MovieClip.prototype.drawCircle 函数,通过数学计算确定圆上各点坐标,利用 curveTo 方法绘制。还展示了创建空影片剪辑、设置线条样式和填充颜色,最后调用 drawCircle 函数完成圆形绘制。

// r 为半径,x,y坐标为圆心坐标

MovieClip.prototype.drawCircle = function(r, x, y) {
  this.moveTo(x+r, y);
  a = Math.tan(22.5 * Math.PI/180);
  for (var angle = 45; angle<=360; angle += 45) {
    // endpoint:
    var endx = r*Math.cos(angle*Math.PI/180);
    var endy = r*Math.sin(angle*Math.PI/180);
           // control:
    // (angle-90 is used to give the correct sign)
    var cx =endx + r*a*Math.cos((angle-90)*Math.PI/180);
    var cy =endy + r*a*Math.sin((angle-90)*Math.PI/180);
    this.curveTo(cx+x, cy+y, endx+x, endy+y);
  }
}

// 开始画圆
var circle_mc = this.createEmptyMovieClip("circle", 1);
circle_mc.lineStyle(1, 0xaaaaaa, 100);
circle_mc.beginFill(0xcccccc, 100);
circle_mc.drawCircle(80, 100, 100);
circle_mc.endFill();

/* * Copyright (c) 2021, Texas Instruments Incorporated * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * * Neither the name of Texas Instruments Incorporated nor the names of * its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "board.h" #include "Key.h" int32_t L_PWM,R_PWM; int Flag_Stop=1; float ATarget_angle=0,BTarget_angle=0; int main(void) { int i=0; SYSCFG_DL_init(); DL_Timer_startCounter(PWM_0_INST); DL_Timer_startCounter(PWM_1_INST); NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN); delay_ms(500); NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); while (1) { printf("L_Period:%.1lf,R_Period:%.1lf\r\n",ATarget_angle,ATarget_angle);//输出此时设置的设置Period_PWM_Count // delay_ms(500); // R_PWM=Calculate_MB_Angle_PWM(BTarget_angle); // Set_PWM(L_PWM,R_PWM);//PWM波驱动电机 // delay_ms(500); } } //5ms定时中断 void TIMER_0_INST_IRQHandler(void) { if(DL_TimerA_getPendingInterrupt(TIMER_0_INST)) { if(DL_TIMER_IIDX_ZERO) { // //LED_Flash(100);//led闪烁 Key();//获取当前BLS按键状态,单击后电机转过45° // //L_PWM=Calculate_MA_Angle_PWM(ATarget_angle); R_PWM=Calculate_MB_Angle_PWM(BTarget_angle); Set_PWM(L_PWM,R_PWM);//PWM波驱动电机 //R_PWM=Calculate_target(180);//设置电机每分钟60转 //Set_PWM(L_PWM,R_PWM);//PWM波驱动电机 } } } 这是我代码的主函数#include "motor.h" #include <math.h> #include <stdlib.h> // --- 硬件参数 #define STEPS_PER_REVOLUTION 200 // 电机物理参数: 电机转一圈(360)需要的步数。1.8度步进角 = 360/1.8 = 200步。 #define MICROSTEPS 16 // 驱动器参数: 驱动器设置的细分数。细分越高,转动越平滑,但对单片机性能要求也越高。 #define PWM_CLOCK_FREQ 10000000 // --- 系统参数 --- #define ISR_INTERVAL_S 0.005f // 中断调用周期()。 // --- 恒定转速参数 (修改部分) --- #define CONSTANT_SPEED_RPM 10 // 目标恒定转速,替代原来的快慢速切换 #define START_SPEED_RPM 3 // 启动速度,避免丢步 #define ACCELERATION_TIME_MS 200 // 加速时间200ms #define ACCELERATION_STEPS ((int)(ACCELERATION_TIME_MS / (ISR_INTERVAL_S * 1000))) // 加速需要的中断次数 // 全局常量:预先计算出"每转一度需要多少个脉冲",方便后续直接使用。 // 公式: (总步数/圈 * 细分数) / 360度/圈 const float PULSES_PER_DEGREE = (STEPS_PER_REVOLUTION * MICROSTEPS) / 360.0f; int Calculate_target(int Target) { if (Target==0) return 0;//如果目标值为0,直接返回0失能电机 // 方向由Target的符号决定,数值部分表示转速(RPM) int direction = (Target >= 0) ? 1 : -1; float speed = (float)ABS(Target); // 转速绝对值(RPM) // 计算每转需要的总步数(考虑细分) float stepsPerRevolution = STEPS_PER_REVOLUTION * MICROSTEPS; // 计算每秒需要的步数(SPM: Steps Per Minute -> SPS: Steps Per Second) float stepsPerSecond = (speed * stepsPerRevolution) / 60.0f; // 计算PWM频率(Hz):每秒需要的脉冲数 float pwmFrequency = stepsPerSecond; // 计算对应的PWM周期值(Period_PWM_Count) // 周期值 = PWM时钟频率 / PWM目标频率 - 1 uint16_t periodValue = (uint16_t)((PWM_CLOCK_FREQ / pwmFrequency) - 1); // 确保计算结果在有效范围内(0-65535) if (periodValue > 65535) { periodValue = 65535; // 限制最大值 } // 返回带方向的周期值 return direction * periodValue; } /** * @brief 将 RPM (每分钟转速) 转换为PWM定时器需要的周期计数值(LoadValue) * @param rpm 目标转速 * @return int 返回给PWM定时器设置的周期值。该值越大,PWM频率越低,电机转速越慢。 */ static int rpm_to_pwm_period(int rpm) { // 如果速度为0,不产生脉冲,直接返回0 if (rpm <= 0) return 0; // 1. 将 RPM (转/分钟) 转换为 RPS (转/秒) float rps = (float)rpm / 60.0f; // 2. 将 RPS (转/秒) 转换为 Steps-per-second (脉冲数/秒) float steps_per_second = rps * STEPS_PER_REVOLUTION * MICROSTEPS; // 3. 脉冲数/秒 就是PWM的频率(Hz) // 4. 根据PWM频率和时钟频率,计算定时器周期值。公式: Period = ClockFreq / PwmFreq - 1 return (int)((PWM_CLOCK_FREQ / steps_per_second) - 1); } /** * @brief 计算电机A()为到达目标角度所需要的PWM控制值 - 恒定转速版本(带加速) * @param target_angle 电机A的目标绝对角度 (单位: 度) * @return int 带符号的PWM周期值。 * - 正数: 表示正转,数值为PWM周期值。 * - 负数: 表示反转,数值的绝对值为PWM周期值。 * - 0: 表示停止。 */ int Calculate_MA_Angle_PWM(float target_angle) { // 电机A的私有状态变量。使用'static'关键字,使得它们的值在函数多次调用之间得以保持。 static float s_ma_current_target_angle = -999.0f; // 初始化为特殊值,避免启动误动作 static float s_ma_current_assumed_angle = 0.0f; // 内部追踪的电机当前假定位置(伪闭环) static long s_ma_pulses_remaining = 0; // 本次运动还剩余多少脉冲需要发送 static int s_ma_direction = 0; // 运动方向: 1=正, -1=反, 0=停 static float s_ma_pulse_accumulator = 0.0f; // 脉冲累加器,用于补偿浮点运算的精度损失 static int s_ma_acceleration_counter = 0; // 加速计数器 static int s_ma_first_call = 1; // 首次调用标志 // 首次调用特殊处理 - 避免启动震动 if (s_ma_first_call) { s_ma_first_call = 0; s_ma_current_target_angle = target_angle; s_ma_current_assumed_angle = target_angle; // 假设启动时已经在目标位置 return 0; // 首次调用不产生任何运动 } // 步骤 1: 检查目标角度是否发生变化。如果变了,就规划一次新的运动。 if (target_angle != s_ma_current_target_angle) { s_ma_current_target_angle = target_angle; // 锁定新目标 // 从上一次运动的终点,计算到新目标需要移动的角度 float angle_to_move = target_angle - s_ma_current_assumed_angle; // 如果移动角度太小(小于0.1度),直接跳转,不执行运动 if (fabs(angle_to_move) < 0.1f) { s_ma_current_assumed_angle = target_angle; s_ma_pulses_remaining = 0; s_ma_direction = 0; return 0; } // 计算总脉冲数 s_ma_pulses_remaining = (long)(fabs(angle_to_move) * PULSES_PER_DEGREE); // 重置脉冲累加器和加速计数器 s_ma_pulse_accumulator = 0.0f; s_ma_acceleration_counter = 0; // 根据移动角度的正负,确定方向 s_ma_direction = (angle_to_move > 0) ? 1 : ((angle_to_move < 0) ? -1 : 0); } // 步骤 2: 如果还有脉冲需要发送,就执行运动控制逻辑。 if (s_ma_pulses_remaining > 0) { // 计算当前应该使用的转速(平滑加速) int current_speed_rpm; if (s_ma_acceleration_counter < ACCELERATION_STEPS) { // 加速阶段:从启动速度线性加速到目标速度 float acceleration_progress = (float)s_ma_acceleration_counter / ACCELERATION_STEPS; current_speed_rpm = (int)(START_SPEED_RPM + (CONSTANT_SPEED_RPM - START_SPEED_RPM) * acceleration_progress); s_ma_acceleration_counter++; } else { // 恒速阶段 current_speed_rpm = CONSTANT_SPEED_RPM; } // 根据当前速度,计算在这个中断周期内(5ms),理论上应发送多少个脉冲(可能是小数) float steps_per_second = (current_speed_rpm * (STEPS_PER_REVOLUTION * MICROSTEPS)) / 60.0f; float pulses_to_send_float = steps_per_second * ISR_INTERVAL_S; // 使用累加器,将每次计算出的小数脉冲累加起来 s_ma_pulse_accumulator += pulses_to_send_float; // 从累加器中取出整数部分作为本次中断实际要发送的脉冲数 long pulses_this_interval = (long)s_ma_pulse_accumulator; if (pulses_this_interval > 0) { s_ma_pulse_accumulator -= pulses_this_interval; // 累加器减去已发送的部分,保留小数 s_ma_pulses_remaining -= pulses_this_interval; // 总剩余脉冲数也减去相应部分 } // 步骤 3: 检查运动是否完成 if (s_ma_pulses_remaining <= 0) { s_ma_pulses_remaining = 0; // 清零,防止负数 s_ma_direction = 0; // 停止 s_ma_acceleration_counter = 0; // 重置加速计数器 // 到达目标!更新电机的假定位置为当前目标位置。这是伪闭环的关键。 s_ma_current_assumed_angle = s_ma_current_target_angle; return 0; // 返回0,命令Set_PWM函数停止电机 } // 如果运动未完成,返回带方向的速度指令 return s_ma_direction * rpm_to_pwm_period(current_speed_rpm); } // 如果没有运动(已到达或无新指令),返回0 return 0; } /** * @brief 计算电机B()为到达目标角度所需要的PWM控制值 - 恒定转速版本(带加速) * @note 此函数逻辑与Calculate_MA_Angle_PWM完全相同,但使用独立的static变量, * 从而实现对电机B的独立状态追踪和控制。 */ int Calculate_MB_Angle_PWM(float target_angle) { // 电机B的私有状态变量 (与电机A的完全独立) static float s_mb_current_target_angle = -999.0f; // 初始化为特殊值 static float s_mb_current_assumed_angle = 0.0f; static long s_mb_pulses_remaining = 0; static int s_mb_direction = 0; static float s_mb_pulse_accumulator = 0.0f; static int s_mb_acceleration_counter = 0; static int s_mb_first_call = 1; // 首次调用标志 // 首次调用特殊处理 - 避免启动震动 if (s_mb_first_call) { s_mb_first_call = 0; s_mb_current_target_angle = target_angle; s_mb_current_assumed_angle = target_angle; // 假设启动时已经在目标位置 return 0; // 首次调用不产生任何运动 } if (target_angle != s_mb_current_target_angle) { s_mb_current_target_angle = target_angle; float angle_to_move = target_angle - s_mb_current_assumed_angle; // 如果移动角度太小,直接跳转 if (fabs(angle_to_move) < 0.1f) { s_mb_current_assumed_angle = target_angle; s_mb_pulses_remaining = 0; s_mb_direction = 0; return 0; } s_mb_pulses_remaining = (long)(fabs(angle_to_move) * PULSES_PER_DEGREE); s_mb_pulse_accumulator = 0.0f; s_mb_acceleration_counter = 0; s_mb_direction = (angle_to_move > 0) ? 1 : ((angle_to_move < 0) ? -1 : 0); } if (s_mb_pulses_remaining > 0) { // 计算当前应该使用的转速(平滑加速) int current_speed_rpm; if (s_mb_acceleration_counter < ACCELERATION_STEPS) { // 加速阶段:从启动速度线性加速到目标速度 float acceleration_progress = (float)s_mb_acceleration_counter / ACCELERATION_STEPS; current_speed_rpm = (int)(START_SPEED_RPM + (CONSTANT_SPEED_RPM - START_SPEED_RPM) * acceleration_progress); s_mb_acceleration_counter++; } else { // 恒速阶段 current_speed_rpm = CONSTANT_SPEED_RPM; } float steps_per_second = (current_speed_rpm * (STEPS_PER_REVOLUTION * MICROSTEPS)) / 60.0f; float pulses_to_send_float = steps_per_second * ISR_INTERVAL_S; s_mb_pulse_accumulator += pulses_to_send_float; long pulses_this_interval = (long)s_mb_pulse_accumulator; if (pulses_this_interval > 0) { s_mb_pulse_accumulator -= pulses_this_interval; s_mb_pulses_remaining -= pulses_this_interval; } if (s_mb_pulses_remaining <= 0) { s_mb_pulses_remaining = 0; s_mb_direction = 0; s_mb_acceleration_counter = 0; // 重置加速计数器 s_mb_current_assumed_angle = s_mb_current_target_angle; return 0; } return s_mb_direction * rpm_to_pwm_period(current_speed_rpm); } return 0; } // ============================================================================== // 5. 底层硬件驱动函数 // ============================================================================== /** * @brief 步进电机底层驱动函数 - 改进版(避免频繁使能/失能) * @param L_Target 左电机(A)的控制指令,由Calculate_MA_Angle_PWM()生成。 * @param R_Target 右电机(B)的控制指令,由Calculate_MB_Angle_PWM()生成。 * @details * - 根据指令值的正负号设置电机方向。 * - 根据指令值的绝对值设置PWM频率(即电机速度)。 * - 指令值为0时,停止PWM输出但保持使能,通过设置占空比为0来完全停止脉冲。 */ void Set_PWM(int L_Target, int R_Target) { // --- 左电机(A)控制 --- if(L_Target > 0) // 指令为正数: 正转 { DL_GPIO_setPins(EN_PORT, EN_MA_PIN); // 1. 打开电机A的使能 DL_GPIO_setPins(DIR_PORT, DIR_A_PIN); // 2. 设置电机A的方向为正转 DL_TimerA_setLoadValue(PWM_0_INST, abs(L_Target)); // 3. 设置PWM周期,决定转速 DL_Timer_setCaptureCompareValue(PWM_0_INST, abs(L_Target/2), GPIO_PWM_0_C0_IDX); // 4. 设置比较值,产生50%占空比方波 } else if(L_Target < 0) // 指令为负数: 反转 { DL_GPIO_setPins(EN_PORT, EN_MA_PIN); // 1. 打开电机A的使能 DL_GPIO_clearPins(DIR_PORT, DIR_A_PIN); // 2. 设置电机A的方向为反转 DL_TimerA_setLoadValue(PWM_0_INST, abs(L_Target)); DL_Timer_setCaptureCompareValue(PWM_0_INST, abs(L_Target/2), GPIO_PWM_0_C0_IDX); } else // 指令为0: 停止但保持使能 { DL_GPIO_setPins(EN_PORT, EN_MA_PIN); // 保持电机A的使能,不失能 // 通过设置占空比为0来完全停止PWM输出,没有脉冲产生 DL_Timer_setCaptureCompareValue(PWM_0_INST, 0, GPIO_PWM_0_C0_IDX); // 占空比为0,完全无输出 // 周期值可以保持之前的值,或者设置一个固定值 DL_TimerA_setLoadValue(PWM_0_INST, 1000); // 设置一个固定的周期值 } // --- 右电机(B)控制 --- if(R_Target > 0) // 指令为正数: 正转 { DL_GPIO_setPins(EN_PORT, EN_MB_PIN); DL_GPIO_setPins(DIR_PORT, DIR_B_PIN); DL_TimerG_setLoadValue(PWM_1_INST, abs(R_Target)); DL_Timer_setCaptureCompareValue(PWM_1_INST, abs(R_Target/2), GPIO_PWM_1_C1_IDX); } else if(R_Target < 0) // 指令为负数: 反转 { DL_GPIO_setPins(EN_PORT, EN_MB_PIN); DL_GPIO_clearPins(DIR_PORT, DIR_B_PIN); DL_TimerG_setLoadValue(PWM_1_INST, abs(R_Target)); DL_Timer_setCaptureCompareValue(PWM_1_INST, abs(R_Target/2), GPIO_PWM_1_C1_IDX); } else // 指令为0: 停止但保持使能 { DL_GPIO_setPins(EN_PORT, EN_MB_PIN); // 保持电机B的使能,不失能 // 通过设置占空比为0来完全停止PWM输出 DL_Timer_setCaptureCompareValue(PWM_1_INST, 0, GPIO_PWM_1_C1_IDX); // 占空比为0,完全无输出 DL_TimerG_setLoadValue(PWM_1_INST, 1000); // 设置一个固定的周期值 } } 这是步进电机驱动函数,现在我要控制它转动角度,但加上按键控制会变得很慢#include "key.h" extern float ATarget_angle,BTarget_angle; /************************************************************************** Function: Key scan Input : Double click the waiting time Output : 0:No action;1:click;2:Double click 函数功能:按键扫描 入口参数:双击等待时间 返回 值:按键状态 0:无动作 1:单击 2:双击 **************************************************************************/ u8 click_N_Double (u8 time) { static u8 flag_key,count_key,double_key=0; static u16 count_single,Forever_count; if(DL_GPIO_readPins(KEY_PORT,KEY_key_PIN)>0) Forever_count++; //长按标志位未置1 else Forever_count=0; if((DL_GPIO_readPins(KEY_PORT,KEY_key_PIN)>0)&&0==flag_key) flag_key=1; //第一次按下 if(0==count_key) { if(flag_key==1) { double_key++; count_key=1; //标记按下一次 } if(double_key==3) { //按下两次 double_key=0; count_single=0; return 2; //双击执行的指令 } } if(0==DL_GPIO_readPins(KEY_PORT,KEY_key_PIN)) flag_key=0,count_key=0; if(1==double_key) { count_single++; if(count_single>time&&Forever_count<time) { double_key=0; count_single=0; //超时不标记为双击 return 1;//单击执行的指令 } if(Forever_count>time) { double_key=0; count_single=0; } } return 0; } /************************************************************************** Function: Long press detection Input : none Output : 0:No action;1:Long press for 2 seconds; 函数功能:长按检测 入口参数:无 返回 值:按键状态 0:无动作 1:长按2s **************************************************************************/ u8 Long_Press(void) { static u16 Long_Press_count,Long_Press; if(Long_Press==0&&KEY==0) Long_Press_count++; //长按标志位未置1 else Long_Press_count=0; if(Long_Press_count>200) //10ms扫描一次 { Long_Press=1; Long_Press_count=0; return 1; } if(Long_Press==1) //长按标志位置1 { Long_Press=0; } return 0; } void Key(void) { u8 tmp,tmp2; tmp=click_N_Double(50); if(tmp==1)//单击电机转过45° { ATarget_angle+=45; BTarget_angle+=90; } } 这是按键代码,现在我想改一个方案,解决这个问题,并且能控制舵机画出一个·半径为6cm的圆
08-01
#encoding: UTF-8 #!/usr/bin/env python2 import cv2 import os import numpy as np import time import rospy from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from geometry_msgs.msg import Twist from actionlib.action_client import GoalManager from pymycobot.mycobot import MyCobot from opencv_yolo import yolo from VideoCapture import FastVideoCapture import math import basic import argparse class GrabParams(object): x_bias = 0 # 在右侧抓取时调整的起始值 y_bias = 0 # 在右侧抓取时调整的起始值 height_bias = 155 # 控制高度偏移 grab_direct = "front" coords_ready = [200, -30, 240, -175, 0, -134] GRAB_MOVE_SPEED = 20 # 抓取移动的速度 debug = True # 调试开关 IMG_SIZE = 640 # 图像大小 done = False # 完成状态 cap_num = 2 # 捕捉编号 usb_dev = "/dev/arm" # USB设备路径 baudrate = 115200 # 通信波特率 # 创建 GrabParams 类的实例 grabParams = GrabParams() rospy.init_node('my_node_name') parser = argparse.ArgumentParser(description='manual to this script') parser.add_argument("--debug", type=bool, default="True") args = parser.parse_args() height_bias = grabParams.height_bias coords = [6.0, -175.7, 278.5, 88.52, 50.23, 2.81] done = grabParams.done CLASSES = ("apple", "clock", "banana","cat ","bird ") #[0,1,2,3,4][43.7, -233.3, 313.9, -99.55, -48.07, -170.41] class Detect_marker(object): def __init__(self): super(Detect_marker, self).__init__() self.mc = MyCobot(grabParams.usb_dev, grabParams.baudrate) self.mc.power_on() self.yolo = yolo() # parameters to calculate camera clipping parameters self.x1 = self.x2 = self.y1 = self.y2 = 0 # use to calculate coord between cube and mycobot self.sum_x1 = self.sum_x2 = self.sum_y2 = self.sum_y1 = 0 # The coordinates of the grab center point relative to the mycobot # The coordinates of the cube relative to the mycobot self.c_x, self.c_y = grabParams.IMG_SIZE/2, grabParams.IMG_SIZE/2 # The ratio of pixels to actual values self.ratio = 0.21 # Grasping motion def move_1(self): mc = MyCobot("/dev/arm",115200) mc.power_on() mc.set_color(0,0,255)#blue, arm is busy # 抓取位置 mc.send_angles([-89.81, -47.19, 48.07, -1.23, 5.44, 130.95],40) time.sleep(2) mc.send_angles([-85.07, -79.01, 103.0, -27.15, -2.46, 130.78],25) time.sleep(2) mc.set_gripper_value(1,35) time.sleep(1) mc.send_angles([-90.43, -78.22,120, -15.64, 7.99, 130.78],40) time.sleep(1) mc.send_angles([0, -78.22,120, -15.64, 7.99, 130.78],40) time.sleep(1) mc.send_angles([39.99, -89.82, 55, -55.89, -4.57, -104.32],40) done = True print("Done") self.mc.set_color(0,255,0)#green, arm is free def move_2(self): mc = MyCobot("/dev/arm",115200) mc.power_on() mc.set_color(0,0,255)#blue, arm is busy mc.send_angles([-89.81, -47.19, 48.07, -1.23, 5.44, 130.95],40) time.sleep(2) mc.send_angles([-85.07, -79.01, 103.0, -27.15, -2.46, 130.78],25) time.sleep(2) mc.set_gripper_value(1,35) time.sleep(1) mc.send_angles([-90.43, -78.22,120, -15.64, 7.99, 130.78],40) time.sleep(1) mc.send_angles([0, -78.22,120, -15.64, 7.99, 130.78],40) time.sleep(1) mc.send_angles([-5, -93.69, 54.66, -51.5, 14.23, -104.32],40) # 放置位置 self.mc.set_color(0,255,0)#green, arm is free # init mycobot def init_mycobot(self): angles = [0, -78.22,120, -15.64, 7.99, 130.78] self.mc.send_angles(angles,30) basic.grap(False) self.mc.send_angles([-89.12, -65.56, 140.36, -78.31, 4.74, 131.66],30) def get_position(self, x, y): # print "self.ratio: ", self.ratio # return (-(x - self.c_x)*self.ratio), (-(y - self.c_y)*self.ratio) wx = wy = 0 if grabParams.grab_direct == "front": wx = (self.c_x - x) * self.ratio wy = (self.c_y - y) * self.ratio return wx, wy def transform_frame(self, frame): frame, ratio, (dw, dh) = self.yolo.letterbox(frame, (grabParams.IMG_SIZE, grabParams.IMG_SIZE)) return frame #图像处理,适配物体识别 def transform_frame_128(self, frame): frame, ratio, (dw, dh) = self.yolo.letterbox(frame, (128, 128)) return frame # detect object def obj_detect(self, img): x=y=0 img_ori = img img_ori = self.transform_frame(img) img = self.transform_frame_128(img) ima = cv2.flip(cv2.transpose(img),0) #加载模型 net = cv2.dnn.readNetFromONNX("/home/robuster/beetle_ai/scripts/beetle_obj.onnx") t1 = time.time() #输入数据处理 blob = cv2.dnn.blobFromImage(img, 1 / 255.0, (128, 128), [0, 0, 0], swapRB=True, crop=False) net.setInput(blob) #推理 outputs = net.forward(net.getUnconnectedOutLayersNames())[0] #获得识别结果 boxes, classes, scores = self.yolo.yolov5_post_process_simple(outputs) t2 = time.time() # img_0 = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) if boxes is not None: class_index = int(classes[0]) # 假设检测到的类索引 label_detected = CLASSES[class_index].strip() boxes = boxes*5 self.yolo.draw_single(img_ori, boxes[0], scores[0], classes[0]) left, top, right, bottom = boxes[0] x = int((left+right)/2) y = int((top+bottom)/2) subt=left-right print('x='+str(x), 'y='+str(y)) print('left-right='+str(left-right)) self.show_image(img_ori) # Print time (inference-only) # print("time: " + str(t2-t1) + "s") if x+y > 0: return x, y,label_detected,subt else: return None def run(self): self.mc.set_color(0,0,255)#blue, arm is busy self.init_mycobot() def show_image(self, img): if grabParams.debug and args.debug: cv2.imshow("figure", img) cv2.waitKey(50) if __name__ == "__main__": detect = Detect_marker() detect.run() time.sleep(2) cap = FastVideoCapture(2) init_num = 0 nparams = 0 num = 0 miss = 0 counts = 20 rate = rospy.Rate(counts) # 20hz while counts > 0 and True: move_cmd = Twist() pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) move_cmd.linear.x = 0.02 time.sleep(0.1) pub.publish(move_cmd) rate.sleep() frame = cap.read() time.sleep(0.1) frame = cv2.flip(cv2.transpose(frame),0) # counts -= 1 detect_result = detect.obj_detect(frame) # if cv2.waitKey(1)==ord('q'): # break if detect_result is None: continue else: x, y ,label,subt= detect_result if abs(320-x)>30: continue if label == "clock" : detect.move_1() else: detect.move_2() cap.close() break 为我解释每一行意思
10-13
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值