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@0:679aca73f9c0, 2021-07-08 (annotated)

Committer:
yuki0108
Date:
Thu Jul 08 06:31:54 2021 +0000
Revision:
0:679aca73f9c0
Child:
1:64229388d55c
7.08

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 0:679aca73f9c0 68 void speedControll()
yuki0108 0:679aca73f9c0 69 {
yuki0108 0:679aca73f9c0 70 motor.Sc(target_speed);
yuki0108 0:679aca73f9c0 71 // saveData();
yuki0108 0:679aca73f9c0 72 // saveOmega();
yuki0108 0:679aca73f9c0 73 }
yuki0108 0:679aca73f9c0 74 void angleControll()
yuki0108 0:679aca73f9c0 75 {
yuki0108 0:679aca73f9c0 76 motor.AcOmega(target_rad);
yuki0108 0:679aca73f9c0 77 // saveData();
yuki0108 0:679aca73f9c0 78 // saveOmega();
yuki0108 0:679aca73f9c0 79 }
yuki0108 0:679aca73f9c0 80
yuki0108 0:679aca73f9c0 81 void displayData()
yuki0108 0:679aca73f9c0 82 {
yuki0108 0:679aca73f9c0 83 for(int i=0; i<omega_count; i++) {
yuki0108 0:679aca73f9c0 84 printf("%f\t%f\t%f\r\n",i*DELTA_T,data_saved[i],omega_saved[i]);
yuki0108 0:679aca73f9c0 85 wait(0.01);
yuki0108 0:679aca73f9c0 86 }
yuki0108 0:679aca73f9c0 87 omega_count=0;
yuki0108 0:679aca73f9c0 88 data_count=0;
yuki0108 0:679aca73f9c0 89 }
yuki0108 0:679aca73f9c0 90
yuki0108 0:679aca73f9c0 91 double initial_duty=0;
yuki0108 0:679aca73f9c0 92 void inputDuty()
yuki0108 0:679aca73f9c0 93 {
yuki0108 0:679aca73f9c0 94 motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 95 ec.calOmega();
yuki0108 0:679aca73f9c0 96 saveData();
yuki0108 0:679aca73f9c0 97 saveOmega();
yuki0108 0:679aca73f9c0 98 }
yuki0108 0:679aca73f9c0 99 void initialThrow() //バックラッシュ対策
yuki0108 0:679aca73f9c0 100 {
yuki0108 0:679aca73f9c0 101 double dead_time=0;
yuki0108 0:679aca73f9c0 102 // if(target_speed<9) {
yuki0108 0:679aca73f9c0 103 // initial_duty=0.4;
yuki0108 0:679aca73f9c0 104 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 105 // dead_time=0.020;
yuki0108 0:679aca73f9c0 106 // } else if(target_speed<13) {
yuki0108 0:679aca73f9c0 107 // initial_duty=0.30;
yuki0108 0:679aca73f9c0 108 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 109 // dead_time=0.0200;
yuki0108 0:679aca73f9c0 110 // } else if(target_speed<18) {
yuki0108 0:679aca73f9c0 111 // initial_duty=0.30;
yuki0108 0:679aca73f9c0 112 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 113 //// dead_time=0.024;
yuki0108 0:679aca73f9c0 114 // dead_time=0.020;
yuki0108 0:679aca73f9c0 115 // } else if(target_speed<20) {
yuki0108 0:679aca73f9c0 116 // initial_duty=0.63;
yuki0108 0:679aca73f9c0 117 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 118 // dead_time=0.020;
yuki0108 0:679aca73f9c0 119 // } else if(target_speed<21) {
yuki0108 0:679aca73f9c0 120 // initial_duty=0.63;
yuki0108 0:679aca73f9c0 121 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 122 // dead_time=0.031;
yuki0108 0:679aca73f9c0 123 // } else if(target_speed<23) {
yuki0108 0:679aca73f9c0 124 // initial_duty=0.64;
yuki0108 0:679aca73f9c0 125 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 126 // dead_time=0.030;
yuki0108 0:679aca73f9c0 127 // } else if(target_speed<25) {
yuki0108 0:679aca73f9c0 128 // initial_duty=0.70;
yuki0108 0:679aca73f9c0 129 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 130 // dead_time=0.030;
yuki0108 0:679aca73f9c0 131 // } else if(target_speed<27) {
yuki0108 0:679aca73f9c0 132 // initial_duty=0.75;
yuki0108 0:679aca73f9c0 133 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 134 // dead_time=0.030;
yuki0108 0:679aca73f9c0 135 // } else if(target_speed<29) {
yuki0108 0:679aca73f9c0 136 // initial_duty=0.87;
yuki0108 0:679aca73f9c0 137 // motor.turn(initial_duty);
yuki0108 0:679aca73f9c0 138 // dead_time=0.030;
yuki0108 0:679aca73f9c0 139 // }
yuki0108 0:679aca73f9c0 140
yuki0108 0:679aca73f9c0 141 initial_duty=0.05;
yuki0108 0:679aca73f9c0 142 dead_time=0.30;
yuki0108 0:679aca73f9c0 143 ticker.attach(&inputDuty,DELTA_T);
yuki0108 0:679aca73f9c0 144 wait(dead_time);
yuki0108 0:679aca73f9c0 145 ticker.detach();
yuki0108 0:679aca73f9c0 146
yuki0108 0:679aca73f9c0 147 }
yuki0108 0:679aca73f9c0 148 int main ()
yuki0108 0:679aca73f9c0 149 {
yuki0108 0:679aca73f9c0 150
yuki0108 0:679aca73f9c0 151 NVIC_SetPriority(TIMER3_IRQn, 5);
yuki0108 0:679aca73f9c0 152 /////////////////////////////////////////////////////////////////////////////
yuki0108 0:679aca73f9c0 153
yuki0108 0:679aca73f9c0 154 motor.setEquation(0.0226,0.0039,-0.0226,0.0044);//MAXON_17.2
yuki0108 0:679aca73f9c0 155
yuki0108 0:679aca73f9c0 156 // while(1) {
yuki0108 0:679aca73f9c0 157 // motor.turn(0.1);
yuki0108 0:679aca73f9c0 158 // printf("%d\r\n",ec.getCount());
yuki0108 0:679aca73f9c0 159 // wait(0.5);
yuki0108 0:679aca73f9c0 160 // }
yuki0108 0:679aca73f9c0 161 //////////////////////////////////////////////////////////////////////////////
yuki0108 0:679aca73f9c0 162
yuki0108 0:679aca73f9c0 163 int state=0;
yuki0108 0:679aca73f9c0 164
yuki0108 0:679aca73f9c0 165 //printf("READY!!\r\n");
yuki0108 0:679aca73f9c0 166 led2=1;
yuki0108 0:679aca73f9c0 167
yuki0108 0:679aca73f9c0 168 while(1) {
yuki0108 0:679aca73f9c0 169 if(state==0) {
yuki0108 0:679aca73f9c0 170 if(toggle) {
yuki0108 0:679aca73f9c0 171 state++;
yuki0108 0:679aca73f9c0 172 motor.reset();
yuki0108 0:679aca73f9c0 173 target_speed=THROW_SPEED;
yuki0108 0:679aca73f9c0 174
yuki0108 0:679aca73f9c0 175 initialThrow();
yuki0108 0:679aca73f9c0 176 ticker.attach(&speedControll,DELTA_T);
yuki0108 0:679aca73f9c0 177 } else {
yuki0108 0:679aca73f9c0 178 led1=!led1;
yuki0108 0:679aca73f9c0 179 wait(0.5);
yuki0108 0:679aca73f9c0 180 }
yuki0108 0:679aca73f9c0 181 } else if(state==1) {
yuki0108 0:679aca73f9c0 182 if(ec.getDeg()>THROW_FIN) {
yuki0108 0:679aca73f9c0 183 ticker.detach();
yuki0108 0:679aca73f9c0 184 target_rad=convertRad(THROW_FIN);
yuki0108 0:679aca73f9c0 185 ticker.attach(&angleControll,DELTA_T);
yuki0108 0:679aca73f9c0 186 state++;
yuki0108 0:679aca73f9c0 187 timer.reset();
yuki0108 0:679aca73f9c0 188 timer.start();
yuki0108 0:679aca73f9c0 189 }
yuki0108 0:679aca73f9c0 190 } else if(state==2) {
yuki0108 0:679aca73f9c0 191 if(fabs(ec.getDeg()-THROW_FIN)>2.5) {
yuki0108 0:679aca73f9c0 192 timer.reset();
yuki0108 0:679aca73f9c0 193 } else if(timer.read()>0.15) {
yuki0108 0:679aca73f9c0 194 ticker.detach();
yuki0108 0:679aca73f9c0 195 target_speed=0;
yuki0108 0:679aca73f9c0 196 ticker.attach(&speedControll,DELTA_T);
yuki0108 0:679aca73f9c0 197 state++;
yuki0108 0:679aca73f9c0 198 }
yuki0108 0:679aca73f9c0 199 } else if(state==3) {
yuki0108 0:679aca73f9c0 200 if(fabs(ec.getOmega())>1) {
yuki0108 0:679aca73f9c0 201 timer.reset();
yuki0108 0:679aca73f9c0 202 } else if(timer.read()>0.05) {
yuki0108 0:679aca73f9c0 203 ticker.detach();
yuki0108 0:679aca73f9c0 204 target_rad=convertRad(BOTTOM_ANGLE);
yuki0108 0:679aca73f9c0 205 ticker.attach(&angleControll,DELTA_T);
yuki0108 0:679aca73f9c0 206 state++;
yuki0108 0:679aca73f9c0 207 }
yuki0108 0:679aca73f9c0 208 } else if(state==4) {
yuki0108 0:679aca73f9c0 209 if(fabs(ec.getDeg()-BOTTOM_ANGLE)>1) {
yuki0108 0:679aca73f9c0 210 timer.reset();
yuki0108 0:679aca73f9c0 211 } else if(timer.read()>0.05) {
yuki0108 0:679aca73f9c0 212 ticker.detach();
yuki0108 0:679aca73f9c0 213 state++;
yuki0108 0:679aca73f9c0 214
yuki0108 0:679aca73f9c0 215 }
yuki0108 0:679aca73f9c0 216 } else if(state==5) {
yuki0108 0:679aca73f9c0 217 state++;
yuki0108 0:679aca73f9c0 218 motor.stop();
yuki0108 0:679aca73f9c0 219
yuki0108 0:679aca73f9c0 220 } else if(state==6) {
yuki0108 0:679aca73f9c0 221 displayData();
yuki0108 0:679aca73f9c0 222 state++;
yuki0108 0:679aca73f9c0 223 } else if(state==7) {
yuki0108 0:679aca73f9c0 224 // state=0;
yuki0108 0:679aca73f9c0 225 state++;
yuki0108 0:679aca73f9c0 226 wait(2);
yuki0108 0:679aca73f9c0 227 }
yuki0108 0:679aca73f9c0 228 wait_us(10);
yuki0108 0:679aca73f9c0 229
yuki0108 0:679aca73f9c0 230 }
yuki0108 0:679aca73f9c0 231 }