7th_DENSOU / Mbed 2 deprecated NHK2021_Throw

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed CalPID MotorController ros_lib_melodic Encoder

main.cpp@4:51b3e070cd89, 2021-07-25 (annotated)

Committer:
koheim
Date:
Sun Jul 25 13:58:43 2021 +0000
Revision:
4:51b3e070cd89
Parent:
3:7c90e5389b84 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0108 0:679aca73f9c0 1 #include "mbed.h"
yuki0108 0:679aca73f9c0 2 #include "EC.h"
yuki0108 0:679aca73f9c0 3 #define RESOLUTION 500
yuki0108 0:679aca73f9c0 4 #include "CalPID.h"
yuki0108 0:679aca73f9c0 5 #include "MotorController.h"
koheim 4:51b3e070cd89 6 #include <ros.h>
koheim 4:51b3e070cd89 7 #include <geometry_msgs/Point.h>
yuki0108 0:679aca73f9c0 8 #define DELTA_T 0.001
yuki0108 0:679aca73f9c0 9
yuki0108 0:679aca73f9c0 10 #define TIME_TURNING 0.8
yuki0108 0:679aca73f9c0 11 #define DUTY_MAX 0.8
yuki0108 0:679aca73f9c0 12 #define OMEGA_MAX 5
yuki0108 0:679aca73f9c0 13
yuki0108 0:679aca73f9c0 14 ////////////////////////////////////////
koheim 4:51b3e070cd89 15 #define THROW_SPEED 11.9
yuki0108 0:679aca73f9c0 16
yuki0108 0:679aca73f9c0 17 ///////////////////////////////////////
koheim 4:51b3e070cd89 18 #define THROW_FIN 95
yuki0108 0:679aca73f9c0 19 #define BOTTOM_ANGLE 0.5
yuki0108 0:679aca73f9c0 20 #define STOP_ANGLE 0.1
yuki0108 0:679aca73f9c0 21
yuki0108 0:679aca73f9c0 22
yuki0108 0:679aca73f9c0 23 DigitalIn toggle(p6,PullUp);
yuki0108 0:679aca73f9c0 24
yuki0108 0:679aca73f9c0 25 Timer timer;
yuki0108 0:679aca73f9c0 26 Timer timer_loop;
yuki0108 0:679aca73f9c0 27 Ticker ticker;
yuki0108 0:679aca73f9c0 28 DigitalOut led1(LED1);
yuki0108 0:679aca73f9c0 29 DigitalOut led2(LED2);
koheim 4:51b3e070cd89 30 DigitalOut led3(LED3);
yuki0108 0:679aca73f9c0 31
yuki0108 0:679aca73f9c0 32 //CalPID speed_pid(0.9281,0,0.0002486,DELTA_T,DUTY_MAX);
yuki0108 0:679aca73f9c0 33 //CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
yuki0108 0:679aca73f9c0 34 //CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);
yuki0108 0:679aca73f9c0 35
yuki0108 0:679aca73f9c0 36 CalPID speed_pid(0.9281,0,0.0003086,DELTA_T,DUTY_MAX);
yuki0108 0:679aca73f9c0 37 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
yuki0108 0:679aca73f9c0 38 CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);
yuki0108 0:679aca73f9c0 39
yuki0108 0:679aca73f9c0 40
yuki0108 0:679aca73f9c0 41 Ec1multi ec(p17,p18,RESOLUTION); //2逓倍用class
oshin1030 3:7c90e5389b84 42 MotorController motor(p21,p22,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);
yuki0108 0:679aca73f9c0 43
koheim 4:51b3e070cd89 44 float shot = 0;
koheim 4:51b3e070cd89 45
koheim 4:51b3e070cd89 46 void throwCallback(const geometry_msgs::Point &throw_state){
koheim 4:51b3e070cd89 47 shot = throw_state.x; //1になったら発射
koheim 4:51b3e070cd89 48 }
yuki0108 0:679aca73f9c0 49
yuki0108 0:679aca73f9c0 50 double convertRad(double degree)
yuki0108 0:679aca73f9c0 51 {
yuki0108 0:679aca73f9c0 52 return degree*M_PI/180.0;
yuki0108 0:679aca73f9c0 53 }
yuki0108 0:679aca73f9c0 54
yuki0108 0:679aca73f9c0 55 float data_saved[2500]= {};
yuki0108 0:679aca73f9c0 56 int data_count=0;
yuki0108 0:679aca73f9c0 57 void saveData()
yuki0108 0:679aca73f9c0 58 {
yuki0108 0:679aca73f9c0 59 if(data_count<150) {
yuki0108 0:679aca73f9c0 60 data_saved[data_count]=ec.getDeg();
yuki0108 0:679aca73f9c0 61 data_count++;
yuki0108 0:679aca73f9c0 62 }
yuki0108 0:679aca73f9c0 63 }
yuki0108 0:679aca73f9c0 64 float target_rad=0;
yuki0108 0:679aca73f9c0 65 float target_speed=0;
yuki0108 0:679aca73f9c0 66 float omega_saved[2500]= {};
yuki0108 0:679aca73f9c0 67 int omega_count=0;
yuki0108 0:679aca73f9c0 68 void saveOmega()
yuki0108 0:679aca73f9c0 69 {
yuki0108 0:679aca73f9c0 70 if(omega_count<150) {
yuki0108 0:679aca73f9c0 71 omega_saved[omega_count]=ec.getOmega();
yuki0108 0:679aca73f9c0 72 omega_count++;
yuki0108 0:679aca73f9c0 73 }
yuki0108 0:679aca73f9c0 74 }
yuki0108 2:5289880d96df 75
yuki0108 2:5289880d96df 76 void setSpeed(double target_rads)
yuki0108 2:5289880d96df 77 {
yuki0108 2:5289880d96df 78 motor.setPDParamSc(0.9281,0.0003086);
yuki0108 2:5289880d96df 79 target_speed=target_rads;
yuki0108 2:5289880d96df 80 }
yuki0108 2:5289880d96df 81 void setAngle(double target_deg)
yuki0108 2:5289880d96df 82 {
oshin1030 3:7c90e5389b84 83 motor.setPDParamSc(0.00100,0.00000070000);
yuki0108 2:5289880d96df 84 target_rad=convertRad(target_deg);
yuki0108 2:5289880d96df 85 }
yuki0108 2:5289880d96df 86
yuki0108 0:679aca73f9c0 87 void speedControll()
yuki0108 0:679aca73f9c0 88 {
yuki0108 0:679aca73f9c0 89 motor.Sc(target_speed);
yuki0108 0:679aca73f9c0 90 // saveData();
yuki0108 0:679aca73f9c0 91 // saveOmega();
yuki0108 0:679aca73f9c0 92 }
yuki0108 0:679aca73f9c0 93 void angleControll()
yuki0108 0:679aca73f9c0 94 {
yuki0108 0:679aca73f9c0 95 motor.AcOmega(target_rad);
yuki0108 0:679aca73f9c0 96 // saveData();
yuki0108 0:679aca73f9c0 97 // saveOmega();
yuki0108 0:679aca73f9c0 98 }
yuki0108 0:679aca73f9c0 99
yuki0108 0:679aca73f9c0 100 void displayData()
yuki0108 0:679aca73f9c0 101 {
yuki0108 0:679aca73f9c0 102 for(int i=0; i<omega_count; i++) {
yuki0108 0:679aca73f9c0 103 printf("%f\t%f\t%f\r\n",i*DELTA_T,data_saved[i],omega_saved[i]);
yuki0108 0:679aca73f9c0 104 wait(0.01);
yuki0108 0:679aca73f9c0 105 }
yuki0108 0:679aca73f9c0 106 omega_count=0;
yuki0108 0:679aca73f9c0 107 data_count=0;
yuki0108 0:679aca73f9c0 108 }
yuki0108 0:679aca73f9c0 109
yuki0108 0:679aca73f9c0 110 double initial_duty=0;
yuki0108 0:679aca73f9c0 111 void inputDuty()
yuki0108 0:679aca73f9c0 112 {
yuki0108 0:679aca73f9c0 113 motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 114 ec.calOmega();
yuki0108 0:679aca73f9c0 115 saveData();
yuki0108 0:679aca73f9c0 116 saveOmega();
yuki0108 0:679aca73f9c0 117 }
yuki0108 0:679aca73f9c0 118 void initialThrow() //バックラッシュ対策
yuki0108 0:679aca73f9c0 119 {
yuki0108 0:679aca73f9c0 120 double dead_time=0;
yuki0108 0:679aca73f9c0 121 // if(target_speed<9) {
yuki0108 0:679aca73f9c0 122 // initial_duty=0.4;
yuki0108 0:679aca73f9c0 123 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 124 // dead_time=0.020;
yuki0108 0:679aca73f9c0 125 // } else if(target_speed<13) {
yuki0108 0:679aca73f9c0 126 // initial_duty=0.30;
yuki0108 0:679aca73f9c0 127 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 128 // dead_time=0.0200;
yuki0108 0:679aca73f9c0 129 // } else if(target_speed<18) {
yuki0108 0:679aca73f9c0 130 // initial_duty=0.30;
yuki0108 0:679aca73f9c0 131 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 132 //// dead_time=0.024;
yuki0108 0:679aca73f9c0 133 // dead_time=0.020;
yuki0108 0:679aca73f9c0 134 // } else if(target_speed<20) {
yuki0108 0:679aca73f9c0 135 // initial_duty=0.63;
yuki0108 0:679aca73f9c0 136 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 137 // dead_time=0.020;
yuki0108 0:679aca73f9c0 138 // } else if(target_speed<21) {
yuki0108 0:679aca73f9c0 139 // initial_duty=0.63;
yuki0108 0:679aca73f9c0 140 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 141 // dead_time=0.031;
yuki0108 0:679aca73f9c0 142 // } else if(target_speed<23) {
yuki0108 0:679aca73f9c0 143 // initial_duty=0.64;
yuki0108 0:679aca73f9c0 144 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 145 // dead_time=0.030;
yuki0108 0:679aca73f9c0 146 // } else if(target_speed<25) {
yuki0108 0:679aca73f9c0 147 // initial_duty=0.70;
yuki0108 0:679aca73f9c0 148 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 149 // dead_time=0.030;
yuki0108 0:679aca73f9c0 150 // } else if(target_speed<27) {
yuki0108 0:679aca73f9c0 151 // initial_duty=0.75;
yuki0108 0:679aca73f9c0 152 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 153 // dead_time=0.030;
yuki0108 0:679aca73f9c0 154 // } else if(target_speed<29) {
yuki0108 0:679aca73f9c0 155 // initial_duty=0.87;
yuki0108 0:679aca73f9c0 156 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 157 // dead_time=0.030;
yuki0108 0:679aca73f9c0 158 // }
yuki0108 0:679aca73f9c0 159
yuki0108 0:679aca73f9c0 160 initial_duty=0.05;
yuki0108 0:679aca73f9c0 161 dead_time=0.30;
yuki0108 0:679aca73f9c0 162 ticker.attach(&inputDuty,DELTA_T);
yuki0108 0:679aca73f9c0 163 wait(dead_time);
yuki0108 0:679aca73f9c0 164 ticker.detach();
koheim 4:51b3e070cd89 165 }
yuki0108 0:679aca73f9c0 166
koheim 4:51b3e070cd89 167 ros::NodeHandle nh;
koheim 4:51b3e070cd89 168 ros::Subscriber<geometry_msgs::Point> sub("/throw",&throwCallback);
yuki0108 0:679aca73f9c0 169
koheim 4:51b3e070cd89 170 int main(){
koheim 4:51b3e070cd89 171 nh.getHardware()->setBaud(115200);
koheim 4:51b3e070cd89 172 nh.initNode();
koheim 4:51b3e070cd89 173 nh.subscribe(sub);
yuki0108 0:679aca73f9c0 174 NVIC_SetPriority(TIMER3_IRQn, 5);
yuki0108 0:679aca73f9c0 175 /////////////////////////////////////////////////////////////////////////////
yuki0108 0:679aca73f9c0 176
yuki0108 0:679aca73f9c0 177 motor.setEquation(0.0226,0.0039,-0.0226,0.0044);//MAXON_17.2
yuki0108 0:679aca73f9c0 178
yuki0108 0:679aca73f9c0 179 // while(1) {
yuki0108 0:679aca73f9c0 180 // motor.turn(0.1);
yuki0108 0:679aca73f9c0 181 // printf("%d\r\n",ec.getCount());
yuki0108 0:679aca73f9c0 182 // wait(0.5);
yuki0108 0:679aca73f9c0 183 // }
yuki0108 0:679aca73f9c0 184 //////////////////////////////////////////////////////////////////////////////
yuki0108 0:679aca73f9c0 185
yuki0108 0:679aca73f9c0 186 int state=0;
yuki0108 0:679aca73f9c0 187
yuki0108 0:679aca73f9c0 188 //printf("READY!!\r\n");
yuki0108 0:679aca73f9c0 189 led2=1;
yuki0108 0:679aca73f9c0 190
yuki0108 0:679aca73f9c0 191 while(1) {
koheim 4:51b3e070cd89 192 nh.spinOnce(); //ROS
yuki0108 0:679aca73f9c0 193 if(state==0) {
koheim 4:51b3e070cd89 194 if(shot == 1) {
koheim 4:51b3e070cd89 195 led3 = 1;
yuki0108 0:679aca73f9c0 196 state++;
yuki0108 0:679aca73f9c0 197 motor.reset();
yuki0108 2:5289880d96df 198 setSpeed(THROW_SPEED);
yuki0108 0:679aca73f9c0 199 initialThrow();
yuki0108 0:679aca73f9c0 200 ticker.attach(&speedControll,DELTA_T);
yuki0108 0:679aca73f9c0 201 } else {
yuki0108 0:679aca73f9c0 202 led1=!led1;
yuki0108 0:679aca73f9c0 203 wait(0.5);
yuki0108 0:679aca73f9c0 204 }
yuki0108 0:679aca73f9c0 205 } else if(state==1) {
yuki0108 0:679aca73f9c0 206 if(ec.getDeg()>THROW_FIN) {
yuki0108 0:679aca73f9c0 207 ticker.detach();
yuki0108 2:5289880d96df 208 setAngle(THROW_FIN+10);
yuki0108 0:679aca73f9c0 209 ticker.attach(&angleControll,DELTA_T);
yuki0108 0:679aca73f9c0 210 state++;
yuki0108 0:679aca73f9c0 211 timer.reset();
yuki0108 0:679aca73f9c0 212 timer.start();
yuki0108 0:679aca73f9c0 213 }
yuki0108 0:679aca73f9c0 214 } else if(state==2) {
yuki0108 1:64229388d55c 215 if(fabs(ec.getDeg()-(THROW_FIN+10))>2.5) {
yuki0108 0:679aca73f9c0 216 timer.reset();
yuki0108 1:64229388d55c 217 } else if(timer.read()>1.30) {
yuki0108 0:679aca73f9c0 218 ticker.detach();
yuki0108 0:679aca73f9c0 219 state++;
yuki0108 0:679aca73f9c0 220 }
yuki0108 0:679aca73f9c0 221 } else if(state==3) {
yuki0108 2:5289880d96df 222 setAngle(BOTTOM_ANGLE);
yuki0108 1:64229388d55c 223 ticker.attach(&angleControll,DELTA_T);
yuki0108 1:64229388d55c 224 state++;
yuki0108 2:5289880d96df 225 } else if(state==4) {
yuki0108 2:5289880d96df 226 if(fabs(ec.getDeg()-BOTTOM_ANGLE)>1) {
yuki0108 2:5289880d96df 227 timer.reset();
yuki0108 2:5289880d96df 228 } else if(timer.read()>0.05) {
yuki0108 2:5289880d96df 229 ticker.detach();
yuki0108 2:5289880d96df 230 state++;
yuki0108 2:5289880d96df 231
yuki0108 2:5289880d96df 232 }
yuki0108 2:5289880d96df 233 } else if(state==5) {
yuki0108 1:64229388d55c 234 state++;
yuki0108 2:5289880d96df 235 motor.stop();
yuki0108 0:679aca73f9c0 236
yuki0108 2:5289880d96df 237 } else if(state==6) {
yuki0108 2:5289880d96df 238 displayData();
yuki0108 2:5289880d96df 239 state++;
yuki0108 2:5289880d96df 240 } else if(state==7) {
yuki0108 0:679aca73f9c0 241 // state=0;
yuki0108 2:5289880d96df 242 state++;
yuki0108 2:5289880d96df 243 wait(2);
yuki0108 2:5289880d96df 244 }
yuki0108 2:5289880d96df 245 wait_us(10);
yuki0108 2:5289880d96df 246
yuki0108 1:64229388d55c 247 }
yuki0108 0:679aca73f9c0 248 }