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@2:5289880d96df, 2021-07-21 (annotated)
- 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?
User | Revision | Line number | New 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 | } |