7th_DENSOU / Mbed 2 deprecated NHK2021_Throw

Dependencies:   mbed CalPID MotorController ros_lib_melodic Encoder

Committer:
yuki0108
Date:
Wed Jul 21 11:00:47 2021 +0000
Revision:
2:5289880d96df
Parent:
1:64229388d55c
Child:
3:7c90e5389b84
7.21

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