ROBOSTEP / Mbed 2 deprecated hybrid_arm_fan

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

Fork of hybrid_arm_main by ROBOSTEP_SHARE

main.cpp@5:08e214191e2a, 2015-12-07 (annotated)

Committer:
hare
Date:
Mon Dec 07 05:53:36 2015 +0000
Revision:
5:08e214191e2a
Parent:
4:5a6706467b10
Child:
6:8db4a07deff0
2015/12/07

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hare 0:6f7d125a0503 1 #include "IR.h"
hare 0:6f7d125a0503 2 #include "count.h"
hare 5:08e214191e2a 3 /*DigitalIn start_sw(p23);
hare 0:6f7d125a0503 4
hare 0:6f7d125a0503 5 PwmOut motor_up(p22);
hare 0:6f7d125a0503 6 PwmOut motor_down(p21);
hare 0:6f7d125a0503 7
hare 0:6f7d125a0503 8 DigitalIn encoder_A(p16);
hare 0:6f7d125a0503 9 DigitalIn encoder_B(p17);
hare 0:6f7d125a0503 10
hare 0:6f7d125a0503 11 AnalogIn sensor(p20);
hare 0:6f7d125a0503 12
hare 0:6f7d125a0503 13 DigitalIn manual_up(p24);
hare 0:6f7d125a0503 14 DigitalIn manual_down(p28);
hare 0:6f7d125a0503 15
hare 0:6f7d125a0503 16 DigitalOut led1(LED1);
hare 0:6f7d125a0503 17 DigitalOut led2(LED2);
hare 0:6f7d125a0503 18 DigitalOut led3(LED3);
hare 0:6f7d125a0503 19
hare 5:08e214191e2a 20 PwmOut fan(p25);*/
hare 5:08e214191e2a 21
hare 5:08e214191e2a 22 DigitalIn start_sw(p21);
hare 5:08e214191e2a 23
hare 5:08e214191e2a 24 PwmOut motor_up(p23);
hare 5:08e214191e2a 25 PwmOut motor_down(p22);
hare 5:08e214191e2a 26
hare 5:08e214191e2a 27 DigitalIn encoder_A(p16);
hare 5:08e214191e2a 28 DigitalIn encoder_B(p17);
hare 0:6f7d125a0503 29
hare 5:08e214191e2a 30 AnalogIn sensor(p20);
hare 5:08e214191e2a 31
hare 5:08e214191e2a 32 DigitalIn manual_up(p28);
hare 5:08e214191e2a 33 DigitalIn manual_down(p24);
hare 5:08e214191e2a 34
hare 5:08e214191e2a 35 DigitalOut led1(LED1);
hare 5:08e214191e2a 36 DigitalOut led2(LED2);
hare 5:08e214191e2a 37 DigitalOut led3(LED3);
hare 5:08e214191e2a 38 DigitalOut led4(LED4);
hare 5:08e214191e2a 39
hare 5:08e214191e2a 40 CAN can1(p30,p29);
hare 5:08e214191e2a 41 PwmOut fan(p25);
hare 2:aa7e8b22b32c 42
hare 0:6f7d125a0503 43 int main(){
hare 1:0f1e77683604 44 motor_up.period_us(20);
hare 1:0f1e77683604 45 motor_down.period_us(20);
hare 0:6f7d125a0503 46 motor_up=0;
hare 0:6f7d125a0503 47 motor_down=0;
hare 2:aa7e8b22b32c 48 printf("lowest calibration\n\r");
hare 1:0f1e77683604 49 led1=1;
hare 0:6f7d125a0503 50 A_s.rise(&A_s_rise);
hare 0:6f7d125a0503 51 A_s.fall(&A_s_fall);
hare 0:6f7d125a0503 52 B_s.rise(&B_s_rise);
hare 0:6f7d125a0503 53 B_s.fall(&B_s_fall);
hare 4:5a6706467b10 54 while(1){
hare 4:5a6706467b10 55 if(manual_up==1 && manual_down==0){
hare 4:5a6706467b10 56 while(1){led1=0;
hare 4:5a6706467b10 57 motor_up=0.2;
hare 4:5a6706467b10 58 motor_down=0;
hare 4:5a6706467b10 59 if(sensor>0.5){motor_up=0.2;motor_down=0;wait(1);break;}}
hare 4:5a6706467b10 60 while(1){led1=1;
hare 4:5a6706467b10 61 motor_up=0;
hare 4:5a6706467b10 62 motor_down=0.15;
hare 4:5a6706467b10 63 if(sensor>0.5){motor_up=0;motor_down=0;break;}}
hare 4:5a6706467b10 64 break;
hare 4:5a6706467b10 65 }
hare 4:5a6706467b10 66 else if(manual_up==0 && manual_down==1){
hare 4:5a6706467b10 67 while(1){motor_up=0;motor_down=0.15;
hare 4:5a6706467b10 68 if(sensor>0.5){motor_up=0;motor_down=0;break;}}}
hare 4:5a6706467b10 69 else if(start_sw==1){
hare 0:6f7d125a0503 70 break;}
hare 4:5a6706467b10 71 else{motor_up=0;
hare 0:6f7d125a0503 72 motor_down=0;}
hare 4:5a6706467b10 73 printf("%f\n\r",(float)sensor);
hare 4:5a6706467b10 74 }
hare 4:5a6706467b10 75 wait(0.3);
hare 2:aa7e8b22b32c 76 ///////////////real calibration ///////////////////
hare 5:08e214191e2a 77 printf("real calibration\n\r");
hare 1:0f1e77683604 78 led2=1;
hare 5:08e214191e2a 79 rotation=0;
hare 5:08e214191e2a 80 int pro=0;
hare 0:6f7d125a0503 81 while(1){
hare 5:08e214191e2a 82 if(rotation>(pro+15) || rotation<(pro-15)){led1 =! led1;
hare 5:08e214191e2a 83 pro=rotation;}
hare 0:6f7d125a0503 84 if(manual_up==1 && manual_down==0){
hare 0:6f7d125a0503 85 motor_down=0;
hare 5:08e214191e2a 86 motor_up=0.15;
hare 0:6f7d125a0503 87 led1=1;
hare 0:6f7d125a0503 88 led2=0;
hare 5:08e214191e2a 89 printf("1 manual_up,%d\n\r",rotation);
hare 1:0f1e77683604 90 }
hare 0:6f7d125a0503 91 else if(manual_up==0 && manual_down==1){
hare 0:6f7d125a0503 92 motor_up=0;
hare 5:08e214191e2a 93 motor_down=0.15;
hare 5:08e214191e2a 94 printf("1 manual_down,%d\n\r",rotation);
hare 0:6f7d125a0503 95 }
hare 2:aa7e8b22b32c 96 else if(start_sw==1){
hare 0:6f7d125a0503 97 break;}
hare 2:aa7e8b22b32c 98 else{motor_up=0;
hare 0:6f7d125a0503 99 motor_down=0;}
hare 5:08e214191e2a 100 //printf("%f\n\r",(float)sensor);//1130 2490 3364
hare 0:6f7d125a0503 101 }
hare 5:08e214191e2a 102 rotation=0;
hare 1:0f1e77683604 103 led1=0;
hare 1:0f1e77683604 104 led2=0;
hare 0:6f7d125a0503 105 motor_up=0;
hare 0:6f7d125a0503 106 motor_down=0;
hare 1:0f1e77683604 107 fan.period_ms(20);
hare 1:0f1e77683604 108 fan.pulsewidth_us(1100);
hare 5:08e214191e2a 109
hare 5:08e214191e2a 110 IR_data.attach(&cal_distance,0.02);
hare 1:0f1e77683604 111 printf("connect battery in 5sec please\n\r");
hare 2:aa7e8b22b32c 112 for(int p=0;p<5;p++){
hare 2:aa7e8b22b32c 113 led1=1;
hare 2:aa7e8b22b32c 114 wait(0.6);
hare 2:aa7e8b22b32c 115 led1=0;
hare 2:aa7e8b22b32c 116 wait(0.4);} // connect ripo in 5s;
hare 2:aa7e8b22b32c 117
hare 2:aa7e8b22b32c 118 printf("4sec is count down\n\r");
hare 2:aa7e8b22b32c 119 for(int p=0;p<4;p++){
hare 1:0f1e77683604 120 led1=1;
hare 2:aa7e8b22b32c 121 led2=1;
hare 2:aa7e8b22b32c 122 wait(0.6);
hare 2:aa7e8b22b32c 123 led2=0;
hare 2:aa7e8b22b32c 124 wait(0.4); }
hare 1:0f1e77683604 125 printf("fan and is running\n\r");
hare 5:08e214191e2a 126 while(1){if(manual_up==1)break;}
hare 5:08e214191e2a 127 int original=avr_distance;
hare 2:aa7e8b22b32c 128 led1=1;
hare 2:aa7e8b22b32c 129 led2=1;
hare 2:aa7e8b22b32c 130 led3=1;
hare 5:08e214191e2a 131 //int original=2350000/IR.read_u16();
hare 0:6f7d125a0503 132 double duty;
hare 0:6f7d125a0503 133 int d;
hare 0:6f7d125a0503 134 int I_d;
hare 0:6f7d125a0503 135 int D_d;
hare 0:6f7d125a0503 136 int pre_d;
hare 1:0f1e77683604 137 float Kp=0.05;
hare 1:0f1e77683604 138 //float Ki=0;
hare 5:08e214191e2a 139 float Kd=0.01;
hare 1:0f1e77683604 140 int x=0;
hare 1:0f1e77683604 141 int fan_power=1100;
hare 1:0f1e77683604 142 int k=0;
hare 2:aa7e8b22b32c 143 int i=0;
hare 0:6f7d125a0503 144 while(1){
hare 5:08e214191e2a 145 //x=2350000/IR.read_u16();
hare 5:08e214191e2a 146 x=avr_distance;
hare 1:0f1e77683604 147 d=x-original;
hare 0:6f7d125a0503 148 I_d +=d;
hare 0:6f7d125a0503 149 D_d=d-pre_d;
hare 5:08e214191e2a 150 ////////////////////fan limit///////////////////
hare 5:08e214191e2a 151 /* if(k==0 && manual_up == 1 && manual_down == 0){
hare 4:5a6706467b10 152 if(i>=2){i=2;}
hare 4:5a6706467b10 153 else{
hare 4:5a6706467b10 154 i++;}
hare 2:aa7e8b22b32c 155 k=1;
hare 2:aa7e8b22b32c 156 }
hare 2:aa7e8b22b32c 157 else if(k==0 && manual_up ==0 && manual_down == 1){
hare 4:5a6706467b10 158 if(i<=0){i=0;}
hare 4:5a6706467b10 159 else{i--;}
hare 1:0f1e77683604 160 k=1;}
hare 5:08e214191e2a 161 else if(rotation>=3100){ led1=0;
hare 2:aa7e8b22b32c 162 led2=0;
hare 2:aa7e8b22b32c 163 led3=0;
hare 2:aa7e8b22b32c 164 motor_up=0;motor_down=0;fan_power=1100;break;}
hare 2:aa7e8b22b32c 165 else if(k==1 && manual_up ==0 && manual_down==0){k=0;}
hare 4:5a6706467b10 166
hare 4:5a6706467b10 167 printf("i=%d,",i);
hare 4:5a6706467b10 168 switch(i){
hare 4:5a6706467b10 169 case 0:
hare 4:5a6706467b10 170 fan_power=1100;
hare 4:5a6706467b10 171 break;
hare 4:5a6706467b10 172 case 1:
hare 5:08e214191e2a 173 fan_power=1500;
hare 4:5a6706467b10 174 break;
hare 4:5a6706467b10 175 case 2:
hare 5:08e214191e2a 176 fan_power=1380;
hare 4:5a6706467b10 177 break;
hare 4:5a6706467b10 178 }
hare 5:08e214191e2a 179 // printf("fan_power=%d\n\r",fan_power);*/
hare 2:aa7e8b22b32c 180 ////////////////////////////////////////////////
hare 5:08e214191e2a 181 fan.pulsewidth_us(1380);
hare 5:08e214191e2a 182 if(rotation>=3100){
hare 0:6f7d125a0503 183 motor_up=0;
hare 0:6f7d125a0503 184 motor_down=0;
hare 1:0f1e77683604 185 led1=0;
hare 1:0f1e77683604 186 led2=0;
hare 1:0f1e77683604 187 led3=0;
hare 5:08e214191e2a 188 fan.pulsewidth_us(1100);
hare 5:08e214191e2a 189 break;}
hare 5:08e214191e2a 190 else if(d<=2 && d>=-1){motor_up=0;motor_down=0;}
hare 4:5a6706467b10 191 else{duty=Kp*d+Kd*D_d;
hare 4:5a6706467b10 192 if(-0.8<duty && duty<-0.6){motor_up=0.6;motor_down=0;}
hare 4:5a6706467b10 193 else if(-0.6<=duty && duty<-0.25){motor_up=-duty;motor_down=0;}
hare 3:2d4049e20ba8 194 else if(-0.25<=duty && duty<0){motor_up=0.25;motor_down=0;}
hare 5:08e214191e2a 195 else if(0<=duty && duty<0.25){motor_up=0;motor_down=0.2;}//0.2
hare 5:08e214191e2a 196 else if(0.25<=duty && duty<0.6){motor_up=0;motor_down=0.7*duty;}//0.7*duty
hare 5:08e214191e2a 197 else if(0.6<=duty && duty<0.8){motor_up=0;motor_down=0;}//0.5
hare 1:0f1e77683604 198 else{motor_up=0;motor_down=0;}
hare 1:0f1e77683604 199 }
hare 2:aa7e8b22b32c 200 pre_d=d;
hare 5:08e214191e2a 201 printf("%d %d %d %f,%f\n\r",x,original,rotation,(float)motor_up,(float)motor_down);
hare 2:aa7e8b22b32c 202 }
hare 2:aa7e8b22b32c 203 motor_up=0;
hare 2:aa7e8b22b32c 204 motor_down=0;
hare 5:08e214191e2a 205 fan.pulsewidth_us(1100);
hare 2:aa7e8b22b32c 206 }
hare 2:aa7e8b22b32c 207 //////////////////////////waiting for start///////////////////////////
hare 1:0f1e77683604 208 /* while(1){
hare 1:0f1e77683604 209 motor_up.period_us(20);
hare 2:aa7e8b22b32c 210 motor_down.period_us(20);
hare 1:0f1e77683604 211 printf("waiting \n\r");
hare 0:6f7d125a0503 212 motor_up=0;
hare 0:6f7d125a0503 213 motor_down=0;
hare 1:0f1e77683604 214 led1=1;
hare 1:0f1e77683604 215 if(start_sw==1){break;}
hare 1:0f1e77683604 216 }
hare 2:aa7e8b22b32c 217 wait_ms(300);*/
hare 1:0f1e77683604 218 //////////////////////////calibration start//////////////////////////
hare 4:5a6706467b10 219 /* if(k==0 && manual_up == 1 && manual_down == 0){
hare 4:5a6706467b10 220 if(fan_power<1500){
hare 4:5a6706467b10 221 fan_power = fan_power + 90;}
hare 4:5a6706467b10 222 else{fan_power=1500;}
hare 4:5a6706467b10 223 data[i]=fan_power;
hare 4:5a6706467b10 224 i++;
hare 4:5a6706467b10 225 k=1;
hare 4:5a6706467b10 226 }
hare 4:5a6706467b10 227 else if(k==0 && manual_up ==0 && manual_down == 1){
hare 4:5a6706467b10 228 if(fan_power > 1100){fan_power = fan_power - 20;}
hare 4:5a6706467b10 229 else{fan_power=1100;}
hare 4:5a6706467b10 230 data[i]=fan_power;
hare 4:5a6706467b10 231 i++;
hare 4:5a6706467b10 232 k=1;}
hare 4:5a6706467b10 233 else if(k==1 && manual_up ==1 && manual_down == 1){
hare 4:5a6706467b10 234 led1=0;
hare 4:5a6706467b10 235 led2=0;
hare 4:5a6706467b10 236 led3=0;
hare 4:5a6706467b10 237 motor_up=0;motor_down=0;fan_power=1100;break;}
hare 4:5a6706467b10 238 else if(k==1 && manual_up ==0 && manual_down==0){k=0;}*/
hare 2:aa7e8b22b32c 239
hare 1:0f1e77683604 240