lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

limit.h@0:7cff999a7f5c, 2020-05-11 (annotated)

Committer:
gohgwaja
Date:
Mon May 11 08:47:18 2020 +0000
Revision:
0:7cff999a7f5c
Child:
1:7b5469bf5994
hahaha

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gohgwaja 0:7cff999a7f5c 1 #ifndef _LIMIT_H_
gohgwaja 0:7cff999a7f5c 2 #define _LIMIT_H_
gohgwaja 0:7cff999a7f5c 3
gohgwaja 0:7cff999a7f5c 4 #define limit_debug_print false
gohgwaja 0:7cff999a7f5c 5 #define start_duty 200
gohgwaja 0:7cff999a7f5c 6
gohgwaja 0:7cff999a7f5c 7 bool limit_find[6]={false,false,false,false,false,false};
gohgwaja 0:7cff999a7f5c 8
gohgwaja 0:7cff999a7f5c 9 DigitalIn limit_sw1(LIMIT_SW1);
gohgwaja 0:7cff999a7f5c 10 DigitalIn limit_sw2(LIMIT_SW2);
gohgwaja 0:7cff999a7f5c 11 DigitalIn limit_sw3(LIMIT_SW3);
gohgwaja 0:7cff999a7f5c 12 DigitalIn limit_sw4(LIMIT_SW4);
gohgwaja 0:7cff999a7f5c 13 DigitalIn limit_sw5(LIMIT_SW5);
gohgwaja 0:7cff999a7f5c 14 DigitalIn limit_sw6(LIMIT_SW6);
gohgwaja 0:7cff999a7f5c 15
gohgwaja 0:7cff999a7f5c 16 #define limit_time 10000
gohgwaja 0:7cff999a7f5c 17
gohgwaja 0:7cff999a7f5c 18 void limit_init()
gohgwaja 0:7cff999a7f5c 19 {
gohgwaja 0:7cff999a7f5c 20 limit_sw1.mode(PullUp);
gohgwaja 0:7cff999a7f5c 21 limit_sw2.mode(PullUp);
gohgwaja 0:7cff999a7f5c 22 limit_sw3.mode(PullUp);
gohgwaja 0:7cff999a7f5c 23 limit_sw4.mode(PullUp);
gohgwaja 0:7cff999a7f5c 24 limit_sw5.mode(PullUp);
gohgwaja 0:7cff999a7f5c 25 limit_sw6.mode(PullUp);
gohgwaja 0:7cff999a7f5c 26 }
gohgwaja 0:7cff999a7f5c 27
gohgwaja 0:7cff999a7f5c 28
gohgwaja 0:7cff999a7f5c 29 double limit_ex_encoder_data[6]={0,};
gohgwaja 0:7cff999a7f5c 30 double diff_encoder_data[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 31 double sum_error[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 32 double motor_duty[6]={-60,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 33 double limit_taget_speed[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 34 double limit_result[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 35 double limit_output[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 36 double limit_error[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 37
gohgwaja 0:7cff999a7f5c 38 double axis123_taget_speed=30;
gohgwaja 0:7cff999a7f5c 39 double axis4_taget_speed=-56;
gohgwaja 0:7cff999a7f5c 40 double axis56_taget_speed=30;
gohgwaja 0:7cff999a7f5c 41
gohgwaja 0:7cff999a7f5c 42 int ccnt=0;
gohgwaja 0:7cff999a7f5c 43 bool limit_check()
gohgwaja 0:7cff999a7f5c 44 {
gohgwaja 0:7cff999a7f5c 45
gohgwaja 0:7cff999a7f5c 46 int sw1 = limit_sw1;
gohgwaja 0:7cff999a7f5c 47 int sw2 = limit_sw2;
gohgwaja 0:7cff999a7f5c 48 int sw3 = limit_sw3;
gohgwaja 0:7cff999a7f5c 49 int sw4 = limit_sw4;
gohgwaja 0:7cff999a7f5c 50 int sw5 = limit_sw5;
gohgwaja 0:7cff999a7f5c 51 int sw6 = limit_sw6;
gohgwaja 0:7cff999a7f5c 52
gohgwaja 0:7cff999a7f5c 53 encoder_read_raw();
gohgwaja 0:7cff999a7f5c 54
gohgwaja 0:7cff999a7f5c 55
gohgwaja 0:7cff999a7f5c 56 if(limit_find[0]==false)
gohgwaja 0:7cff999a7f5c 57 {
gohgwaja 0:7cff999a7f5c 58 if(limit_taget_speed[0]<axis123_taget_speed)
gohgwaja 0:7cff999a7f5c 59 limit_taget_speed[0]+=0.3;
gohgwaja 0:7cff999a7f5c 60
gohgwaja 0:7cff999a7f5c 61 motor_power(0,limit_output[0]);
gohgwaja 0:7cff999a7f5c 62 }else
gohgwaja 0:7cff999a7f5c 63 {
gohgwaja 0:7cff999a7f5c 64 if(limit_taget_speed[0]>0)
gohgwaja 0:7cff999a7f5c 65 limit_taget_speed[0]-=1;
gohgwaja 0:7cff999a7f5c 66
gohgwaja 0:7cff999a7f5c 67 if(limit_taget_speed[0]<0)
gohgwaja 0:7cff999a7f5c 68 {
gohgwaja 0:7cff999a7f5c 69 limit_taget_speed[0]=0;
gohgwaja 0:7cff999a7f5c 70 }
gohgwaja 0:7cff999a7f5c 71 motor_power(0,limit_output[0]);
gohgwaja 0:7cff999a7f5c 72 }
gohgwaja 0:7cff999a7f5c 73
gohgwaja 0:7cff999a7f5c 74 if(limit_find[1]==false)
gohgwaja 0:7cff999a7f5c 75 {
gohgwaja 0:7cff999a7f5c 76 if(limit_taget_speed[1]<axis123_taget_speed)
gohgwaja 0:7cff999a7f5c 77 limit_taget_speed[1]+=0.3;
gohgwaja 0:7cff999a7f5c 78
gohgwaja 0:7cff999a7f5c 79 motor_power(1,limit_output[1]);
gohgwaja 0:7cff999a7f5c 80 }else
gohgwaja 0:7cff999a7f5c 81 {
gohgwaja 0:7cff999a7f5c 82 if(limit_taget_speed[1]>0)
gohgwaja 0:7cff999a7f5c 83 limit_taget_speed[1]-=1;
gohgwaja 0:7cff999a7f5c 84
gohgwaja 0:7cff999a7f5c 85 if(limit_taget_speed[1]<0)
gohgwaja 0:7cff999a7f5c 86 {
gohgwaja 0:7cff999a7f5c 87 limit_taget_speed[1]=0;
gohgwaja 0:7cff999a7f5c 88 }
gohgwaja 0:7cff999a7f5c 89 motor_power(1,limit_output[1]);
gohgwaja 0:7cff999a7f5c 90 }
gohgwaja 0:7cff999a7f5c 91
gohgwaja 0:7cff999a7f5c 92 if(limit_find[2]==false)
gohgwaja 0:7cff999a7f5c 93 {
gohgwaja 0:7cff999a7f5c 94 if(limit_taget_speed[2]<axis123_taget_speed)
gohgwaja 0:7cff999a7f5c 95 limit_taget_speed[2]+=0.3;
gohgwaja 0:7cff999a7f5c 96
gohgwaja 0:7cff999a7f5c 97 motor_power(2,limit_output[2]);
gohgwaja 0:7cff999a7f5c 98 }else
gohgwaja 0:7cff999a7f5c 99 {
gohgwaja 0:7cff999a7f5c 100 if(limit_taget_speed[2]>0)
gohgwaja 0:7cff999a7f5c 101 limit_taget_speed[2]-=1;
gohgwaja 0:7cff999a7f5c 102
gohgwaja 0:7cff999a7f5c 103 if(limit_taget_speed[2]<0)
gohgwaja 0:7cff999a7f5c 104 {
gohgwaja 0:7cff999a7f5c 105 limit_taget_speed[2]=0;
gohgwaja 0:7cff999a7f5c 106 }
gohgwaja 0:7cff999a7f5c 107 motor_power(2,limit_output[2]);
gohgwaja 0:7cff999a7f5c 108 }
gohgwaja 0:7cff999a7f5c 109
gohgwaja 0:7cff999a7f5c 110 if(limit_find[3]==false)
gohgwaja 0:7cff999a7f5c 111 {
gohgwaja 0:7cff999a7f5c 112 if(limit_taget_speed[3]>axis4_taget_speed)
gohgwaja 0:7cff999a7f5c 113 limit_taget_speed[3]-=0.3;
gohgwaja 0:7cff999a7f5c 114
gohgwaja 0:7cff999a7f5c 115 motor_power(3,limit_output[3]);
gohgwaja 0:7cff999a7f5c 116 }else
gohgwaja 0:7cff999a7f5c 117 {
gohgwaja 0:7cff999a7f5c 118 if(limit_taget_speed[3]<0)
gohgwaja 0:7cff999a7f5c 119 limit_taget_speed[3]+=1;
gohgwaja 0:7cff999a7f5c 120
gohgwaja 0:7cff999a7f5c 121 if(limit_taget_speed[3]>0)
gohgwaja 0:7cff999a7f5c 122 {
gohgwaja 0:7cff999a7f5c 123 limit_taget_speed[3]=0;
gohgwaja 0:7cff999a7f5c 124 }
gohgwaja 0:7cff999a7f5c 125 motor_power(3,limit_output[3]);
gohgwaja 0:7cff999a7f5c 126 }
gohgwaja 0:7cff999a7f5c 127
gohgwaja 0:7cff999a7f5c 128 if(ccnt<450)
gohgwaja 0:7cff999a7f5c 129 {
gohgwaja 0:7cff999a7f5c 130 ccnt++;
gohgwaja 0:7cff999a7f5c 131 limit_taget_speed[0]=0;
gohgwaja 0:7cff999a7f5c 132 limit_taget_speed[1]=0;
gohgwaja 0:7cff999a7f5c 133 limit_taget_speed[2]=0;
gohgwaja 0:7cff999a7f5c 134 limit_taget_speed[3]=0;
gohgwaja 0:7cff999a7f5c 135 limit_taget_speed[4]-=0.4;
gohgwaja 0:7cff999a7f5c 136 limit_taget_speed[5]-=0.4;
gohgwaja 0:7cff999a7f5c 137 }
gohgwaja 0:7cff999a7f5c 138
gohgwaja 0:7cff999a7f5c 139 if(limit_find[4]==false)
gohgwaja 0:7cff999a7f5c 140 {
gohgwaja 0:7cff999a7f5c 141 if(limit_taget_speed[4]<axis56_taget_speed)
gohgwaja 0:7cff999a7f5c 142 limit_taget_speed[4]+=0.3;
gohgwaja 0:7cff999a7f5c 143
gohgwaja 0:7cff999a7f5c 144 motor_power(4,-limit_output[4]);
gohgwaja 0:7cff999a7f5c 145 }else
gohgwaja 0:7cff999a7f5c 146 {
gohgwaja 0:7cff999a7f5c 147 if(limit_taget_speed[4]>0)
gohgwaja 0:7cff999a7f5c 148 limit_taget_speed[4]-=1;
gohgwaja 0:7cff999a7f5c 149
gohgwaja 0:7cff999a7f5c 150 if(limit_taget_speed[4]<0)
gohgwaja 0:7cff999a7f5c 151 {
gohgwaja 0:7cff999a7f5c 152 limit_taget_speed[4]=0;
gohgwaja 0:7cff999a7f5c 153 }
gohgwaja 0:7cff999a7f5c 154 motor_power(4,-limit_output[4]);
gohgwaja 0:7cff999a7f5c 155 }
gohgwaja 0:7cff999a7f5c 156 if(limit_find[5]==false)
gohgwaja 0:7cff999a7f5c 157 {
gohgwaja 0:7cff999a7f5c 158 if(limit_taget_speed[5]<axis56_taget_speed)
gohgwaja 0:7cff999a7f5c 159 limit_taget_speed[5]+=0.3;
gohgwaja 0:7cff999a7f5c 160
gohgwaja 0:7cff999a7f5c 161 motor_power(5,-limit_output[5]);
gohgwaja 0:7cff999a7f5c 162 }else
gohgwaja 0:7cff999a7f5c 163 {
gohgwaja 0:7cff999a7f5c 164 if(limit_taget_speed[5]>0)
gohgwaja 0:7cff999a7f5c 165 limit_taget_speed[5]-=1;
gohgwaja 0:7cff999a7f5c 166
gohgwaja 0:7cff999a7f5c 167 if(limit_taget_speed[5]<0)
gohgwaja 0:7cff999a7f5c 168 {
gohgwaja 0:7cff999a7f5c 169 limit_taget_speed[5]=0;
gohgwaja 0:7cff999a7f5c 170 }
gohgwaja 0:7cff999a7f5c 171 motor_power(5,-limit_output[5]);
gohgwaja 0:7cff999a7f5c 172 }
gohgwaja 0:7cff999a7f5c 173
gohgwaja 0:7cff999a7f5c 174
gohgwaja 0:7cff999a7f5c 175 for(int i=0;i<3;i++)
gohgwaja 0:7cff999a7f5c 176 {
gohgwaja 0:7cff999a7f5c 177
gohgwaja 0:7cff999a7f5c 178 diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
gohgwaja 0:7cff999a7f5c 179 limit_ex_encoder_data[i]=encoder_data[i];
gohgwaja 0:7cff999a7f5c 180 limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]);
gohgwaja 0:7cff999a7f5c 181 sum_error[i]+=limit_error[i]*0.2;
gohgwaja 0:7cff999a7f5c 182 limit_result[i]=limit_error[i]*2+sum_error[i]*1;
gohgwaja 0:7cff999a7f5c 183
gohgwaja 0:7cff999a7f5c 184 if(limit_result[i]>200)
gohgwaja 0:7cff999a7f5c 185 limit_result[i]=200;
gohgwaja 0:7cff999a7f5c 186 if(limit_result[i]<-200)
gohgwaja 0:7cff999a7f5c 187 limit_result[i]=-200;
gohgwaja 0:7cff999a7f5c 188
gohgwaja 0:7cff999a7f5c 189 limit_output[i]=motor_duty[i]+limit_result[i];
gohgwaja 0:7cff999a7f5c 190
gohgwaja 0:7cff999a7f5c 191 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 192 pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
gohgwaja 0:7cff999a7f5c 193 }
gohgwaja 0:7cff999a7f5c 194
gohgwaja 0:7cff999a7f5c 195
gohgwaja 0:7cff999a7f5c 196 for(int i=3;i<4;i++)
gohgwaja 0:7cff999a7f5c 197 {
gohgwaja 0:7cff999a7f5c 198
gohgwaja 0:7cff999a7f5c 199 diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
gohgwaja 0:7cff999a7f5c 200 limit_ex_encoder_data[i]=encoder_data[i];
gohgwaja 0:7cff999a7f5c 201 limit_error[i]=-(diff_encoder_data[i]-limit_taget_speed[i]);
gohgwaja 0:7cff999a7f5c 202 sum_error[i]+=limit_error[i]*0.2;
gohgwaja 0:7cff999a7f5c 203 limit_result[i]=limit_error[i]*2+sum_error[i]*1;
gohgwaja 0:7cff999a7f5c 204
gohgwaja 0:7cff999a7f5c 205 if(limit_result[i]>200)
gohgwaja 0:7cff999a7f5c 206 limit_result[i]=200;
gohgwaja 0:7cff999a7f5c 207 if(limit_result[i]<-200)
gohgwaja 0:7cff999a7f5c 208 limit_result[i]=-200;
gohgwaja 0:7cff999a7f5c 209
gohgwaja 0:7cff999a7f5c 210 limit_output[i]=motor_duty[i]+limit_result[i];
gohgwaja 0:7cff999a7f5c 211
gohgwaja 0:7cff999a7f5c 212 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 213 pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
gohgwaja 0:7cff999a7f5c 214 }
gohgwaja 0:7cff999a7f5c 215
gohgwaja 0:7cff999a7f5c 216
gohgwaja 0:7cff999a7f5c 217 for(int i=4;i<6;i++)
gohgwaja 0:7cff999a7f5c 218 {
gohgwaja 0:7cff999a7f5c 219
gohgwaja 0:7cff999a7f5c 220 diff_encoder_data[i]=encoder_data[i]-limit_ex_encoder_data[i];
gohgwaja 0:7cff999a7f5c 221 limit_ex_encoder_data[i]=encoder_data[i];
gohgwaja 0:7cff999a7f5c 222 limit_error[i]=(diff_encoder_data[i]-limit_taget_speed[i]);
gohgwaja 0:7cff999a7f5c 223 sum_error[i]+=limit_error[i]*0.2;
gohgwaja 0:7cff999a7f5c 224 limit_result[i]=limit_error[i]*2+sum_error[i]*1;
gohgwaja 0:7cff999a7f5c 225
gohgwaja 0:7cff999a7f5c 226 if(limit_result[i]>200)
gohgwaja 0:7cff999a7f5c 227 limit_result[i]=200;
gohgwaja 0:7cff999a7f5c 228 if(limit_result[i]<-200)
gohgwaja 0:7cff999a7f5c 229 limit_result[i]=-200;
gohgwaja 0:7cff999a7f5c 230
gohgwaja 0:7cff999a7f5c 231 limit_output[i]=motor_duty[i]+limit_result[i];
gohgwaja 0:7cff999a7f5c 232
gohgwaja 0:7cff999a7f5c 233 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 234 pc.printf("%8.0f(%4.0f)(%8d) ",diff_encoder_data[i],limit_output[i],encoder_data[i]);
gohgwaja 0:7cff999a7f5c 235 }
gohgwaja 0:7cff999a7f5c 236
gohgwaja 0:7cff999a7f5c 237
gohgwaja 0:7cff999a7f5c 238 if(limit_debug_print)
gohgwaja 0:7cff999a7f5c 239 pc.printf("\n\r");
gohgwaja 0:7cff999a7f5c 240
gohgwaja 0:7cff999a7f5c 241
gohgwaja 0:7cff999a7f5c 242
gohgwaja 0:7cff999a7f5c 243 if(sw1==0 && !limit_find[0])
gohgwaja 0:7cff999a7f5c 244 {
gohgwaja 0:7cff999a7f5c 245 limit_find[0]=true;
gohgwaja 0:7cff999a7f5c 246 encoder1.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 247 limit_ex_encoder_data[0]=0;
gohgwaja 0:7cff999a7f5c 248 }
gohgwaja 0:7cff999a7f5c 249
gohgwaja 0:7cff999a7f5c 250 if(sw2==0 && !limit_find[1])
gohgwaja 0:7cff999a7f5c 251 {
gohgwaja 0:7cff999a7f5c 252 limit_find[1]=true;
gohgwaja 0:7cff999a7f5c 253 encoder2.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 254 limit_ex_encoder_data[1]=0;
gohgwaja 0:7cff999a7f5c 255 }
gohgwaja 0:7cff999a7f5c 256
gohgwaja 0:7cff999a7f5c 257 if(sw3==0 && !limit_find[2])
gohgwaja 0:7cff999a7f5c 258 {
gohgwaja 0:7cff999a7f5c 259 limit_find[2]=true;
gohgwaja 0:7cff999a7f5c 260 encoder3.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 261 limit_ex_encoder_data[2]=0;
gohgwaja 0:7cff999a7f5c 262 }
gohgwaja 0:7cff999a7f5c 263
gohgwaja 0:7cff999a7f5c 264 if(sw4==0 && !limit_find[3])
gohgwaja 0:7cff999a7f5c 265 {
gohgwaja 0:7cff999a7f5c 266 limit_find[3]=true;
gohgwaja 0:7cff999a7f5c 267 encoder4.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 268 limit_ex_encoder_data[3]=0;
gohgwaja 0:7cff999a7f5c 269 }
gohgwaja 0:7cff999a7f5c 270 if(sw5==0 && !limit_find[4])
gohgwaja 0:7cff999a7f5c 271 {
gohgwaja 0:7cff999a7f5c 272 limit_find[4]=true;
gohgwaja 0:7cff999a7f5c 273 encoder5.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 274 limit_ex_encoder_data[4]=0;
gohgwaja 0:7cff999a7f5c 275 }
gohgwaja 0:7cff999a7f5c 276 if(sw6==0 && !limit_find[5])
gohgwaja 0:7cff999a7f5c 277 {
gohgwaja 0:7cff999a7f5c 278 limit_find[5]=true;
gohgwaja 0:7cff999a7f5c 279 encoder6.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 280 limit_ex_encoder_data[5]=0;
gohgwaja 0:7cff999a7f5c 281 }
gohgwaja 0:7cff999a7f5c 282
gohgwaja 0:7cff999a7f5c 283 return (limit_find[0] || !motor_onoff[0]) && (limit_find[1] || !motor_onoff[1]) && (limit_find[2] || !motor_onoff[2]) && (limit_find[3] || !motor_onoff[3]) && (limit_find[4] || !motor_onoff[4]) && (limit_find[5] || !motor_onoff[5]);
gohgwaja 0:7cff999a7f5c 284 }
gohgwaja 0:7cff999a7f5c 285
gohgwaja 0:7cff999a7f5c 286 bool limit_check_done=false;
gohgwaja 0:7cff999a7f5c 287 int limit_check_done_cnt=0;
gohgwaja 0:7cff999a7f5c 288 void find_limit()
gohgwaja 0:7cff999a7f5c 289 {
gohgwaja 0:7cff999a7f5c 290 wait_ms(0);
gohgwaja 0:7cff999a7f5c 291
gohgwaja 0:7cff999a7f5c 292 encoder_reset_cnt();
gohgwaja 0:7cff999a7f5c 293
gohgwaja 0:7cff999a7f5c 294 for(int i=0;i<limit_time;i++)
gohgwaja 0:7cff999a7f5c 295 {
gohgwaja 0:7cff999a7f5c 296 if(limit_check())
gohgwaja 0:7cff999a7f5c 297 {
gohgwaja 0:7cff999a7f5c 298 break;
gohgwaja 0:7cff999a7f5c 299 }
gohgwaja 0:7cff999a7f5c 300 reset_check();
gohgwaja 0:7cff999a7f5c 301 wait_ms(1);
gohgwaja 0:7cff999a7f5c 302
gohgwaja 0:7cff999a7f5c 303 }
gohgwaja 0:7cff999a7f5c 304 encoder_read_raw();
gohgwaja 0:7cff999a7f5c 305 motor_power(0,0);
gohgwaja 0:7cff999a7f5c 306 motor_power(1,0);
gohgwaja 0:7cff999a7f5c 307 motor_power(2,0);
gohgwaja 0:7cff999a7f5c 308 motor_power(3,0);
gohgwaja 0:7cff999a7f5c 309 motor_power(4,0);
gohgwaja 0:7cff999a7f5c 310 motor_power(5,0);
gohgwaja 0:7cff999a7f5c 311 }
gohgwaja 0:7cff999a7f5c 312
gohgwaja 0:7cff999a7f5c 313 #endif