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@0:6f7d125a0503, 2015-10-24 (annotated)

Committer:
hare
Date:
Sat Oct 24 00:35:44 2015 +0000
Revision:
0:6f7d125a0503
Child:
1:0f1e77683604
2015/10/24

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 0:6f7d125a0503 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 0:6f7d125a0503 20 PwmOut fan(p25);
hare 0:6f7d125a0503 21
hare 0:6f7d125a0503 22 int main(){
hare 0:6f7d125a0503 23 //////////////////////////waiting for start///////////////////////////
hare 0:6f7d125a0503 24 while(1){
hare 0:6f7d125a0503 25 motor_up.period_us(20);
hare 0:6f7d125a0503 26 motor_down.period_us(20);
hare 0:6f7d125a0503 27 printf("waiting \n\r");
hare 0:6f7d125a0503 28 motor_up=0;
hare 0:6f7d125a0503 29 motor_down=0;
hare 0:6f7d125a0503 30 led1=1;
hare 0:6f7d125a0503 31 if(start_sw==1){break;}
hare 0:6f7d125a0503 32 }
hare 0:6f7d125a0503 33 //////////////////////////calibration start//////////////////////////
hare 0:6f7d125a0503 34 /* while(1){
hare 0:6f7d125a0503 35 motor_down=0.1;
hare 0:6f7d125a0503 36 motor_up=0;
hare 0:6f7d125a0503 37 led1=0;
hare 0:6f7d125a0503 38 led2=1;
hare 0:6f7d125a0503 39 printf("auto calibrating\n\r");
hare 0:6f7d125a0503 40 if(sensor>0.5){
hare 0:6f7d125a0503 41 motor_down=0;
hare 0:6f7d125a0503 42 motor_up=0;
hare 0:6f7d125a0503 43 led2=0;
hare 0:6f7d125a0503 44 printf("over\n\r");
hare 0:6f7d125a0503 45 break;}
hare 0:6f7d125a0503 46 }*/
hare 0:6f7d125a0503 47 //////////////////////manual calibration 1/////////////////
hare 0:6f7d125a0503 48 motor_up=0;
hare 0:6f7d125a0503 49 motor_down=0;
hare 0:6f7d125a0503 50 wait_ms(400);
hare 0:6f7d125a0503 51 printf("asd\n\r");
hare 0:6f7d125a0503 52 A_s.rise(&A_s_rise);
hare 0:6f7d125a0503 53 A_s.fall(&A_s_fall);
hare 0:6f7d125a0503 54 B_s.rise(&B_s_rise);
hare 0:6f7d125a0503 55 B_s.fall(&B_s_fall);
hare 0:6f7d125a0503 56 while(1){
hare 0:6f7d125a0503 57 if(manual_up==1 && manual_down==0){
hare 0:6f7d125a0503 58
hare 0:6f7d125a0503 59 motor_down=0;
hare 0:6f7d125a0503 60 motor_up=0.2;
hare 0:6f7d125a0503 61 led1=1;
hare 0:6f7d125a0503 62 led2=0;
hare 0:6f7d125a0503 63 printf("manual_up,%d\n\r",rotation);
hare 0:6f7d125a0503 64 }
hare 0:6f7d125a0503 65 else if(manual_up==0 && manual_down==1){
hare 0:6f7d125a0503 66 motor_up=0;
hare 0:6f7d125a0503 67 motor_down=0.2;
hare 0:6f7d125a0503 68 printf("manual_down,%d\n\r",rotation);
hare 0:6f7d125a0503 69 }
hare 0:6f7d125a0503 70
hare 0:6f7d125a0503 71 else if(start_sw==1){
hare 0:6f7d125a0503 72 rotation=0;
hare 0:6f7d125a0503 73 break;}
hare 0:6f7d125a0503 74 else{motor_up=0;
hare 0:6f7d125a0503 75 motor_down=0;}
hare 0:6f7d125a0503 76 }
hare 0:6f7d125a0503 77 wait_ms(300);
hare 0:6f7d125a0503 78 ///////////////manual calibration 2///////////////////
hare 0:6f7d125a0503 79 while(1){
hare 0:6f7d125a0503 80 if(manual_up==1 && manual_down==0){
hare 0:6f7d125a0503 81
hare 0:6f7d125a0503 82 motor_down=0;
hare 0:6f7d125a0503 83 motor_up=0.2;
hare 0:6f7d125a0503 84 led1=1;
hare 0:6f7d125a0503 85 led2=0;
hare 0:6f7d125a0503 86 printf("manual_up,%d\n\r",rotation);
hare 0:6f7d125a0503 87 }//9116
hare 0:6f7d125a0503 88 else if(manual_up==0 && manual_down==1){
hare 0:6f7d125a0503 89 motor_up=0;
hare 0:6f7d125a0503 90 motor_down=0.2;
hare 0:6f7d125a0503 91 printf("manual_down,%d\n\r",rotation);
hare 0:6f7d125a0503 92 }
hare 0:6f7d125a0503 93
hare 0:6f7d125a0503 94 else if(start_sw==1){
hare 0:6f7d125a0503 95 break;}
hare 0:6f7d125a0503 96 else{motor_up=0;
hare 0:6f7d125a0503 97 motor_down=0;}
hare 0:6f7d125a0503 98 }
hare 0:6f7d125a0503 99 IR_data.attach(&cal_distance,0.005);
hare 0:6f7d125a0503 100 motor_up=0;
hare 0:6f7d125a0503 101 motor_down=0;
hare 0:6f7d125a0503 102 wait(1);
hare 0:6f7d125a0503 103
hare 0:6f7d125a0503 104 int original=avr_distance;
hare 0:6f7d125a0503 105
hare 0:6f7d125a0503 106 double duty;
hare 0:6f7d125a0503 107 printf("motor controlling\n\r");
hare 0:6f7d125a0503 108 int d;
hare 0:6f7d125a0503 109 int I_d;
hare 0:6f7d125a0503 110 int D_d;
hare 0:6f7d125a0503 111 int pre_d;
hare 0:6f7d125a0503 112 float Kp=0.03;
hare 0:6f7d125a0503 113 float Ki=0;
hare 0:6f7d125a0503 114 float Kd=0.005;
hare 0:6f7d125a0503 115
hare 0:6f7d125a0503 116 while(1){
hare 0:6f7d125a0503 117 //printf("%d %d %f,%f\n\r",avr_distance,original,(float)motor_up,(float)motor_down);
hare 0:6f7d125a0503 118 d=avr_distance-original;
hare 0:6f7d125a0503 119 I_d +=d;
hare 0:6f7d125a0503 120 D_d=d-pre_d;
hare 0:6f7d125a0503 121 if(rotation>=12500){
hare 0:6f7d125a0503 122 motor_up=0;
hare 0:6f7d125a0503 123 motor_down=0;
hare 0:6f7d125a0503 124 led1=1;
hare 0:6f7d125a0503 125 led2=1;
hare 0:6f7d125a0503 126 led3=1;
hare 0:6f7d125a0503 127 break;}
hare 0:6f7d125a0503 128 else if(rotation <=150){
hare 0:6f7d125a0503 129 motor_up=0.2;
hare 0:6f7d125a0503 130 motor_down=0;
hare 0:6f7d125a0503 131 wait(1);}
hare 0:6f7d125a0503 132 if(avr_distance<60){
hare 0:6f7d125a0503 133 motor_up=0.3;
hare 0:6f7d125a0503 134 motor_down=0;
hare 0:6f7d125a0503 135 wait(0.5);}
hare 0:6f7d125a0503 136 if(d<=1 && d>=-1){motor_up=0;motor_down=0;}
hare 0:6f7d125a0503 137
hare 0:6f7d125a0503 138 else {duty=Kp*d+Ki*I_d+Kd*D_d;}
hare 0:6f7d125a0503 139
hare 0:6f7d125a0503 140 if(-0.5<duty && duty<0.5){
hare 0:6f7d125a0503 141 if(0<=duty){
hare 0:6f7d125a0503 142 motor_up=0;
hare 0:6f7d125a0503 143 motor_down=duty;}
hare 0:6f7d125a0503 144 else if(duty<0){
hare 0:6f7d125a0503 145 duty=-duty;
hare 0:6f7d125a0503 146 motor_up=duty;
hare 0:6f7d125a0503 147 motor_down=0;}}
hare 0:6f7d125a0503 148 else{
hare 0:6f7d125a0503 149 motor_up=0;
hare 0:6f7d125a0503 150 motor_down=0;}
hare 0:6f7d125a0503 151 pre_d=d;
hare 0:6f7d125a0503 152 }
hare 0:6f7d125a0503 153
hare 0:6f7d125a0503 154 motor_up=0;
hare 0:6f7d125a0503 155 motor_down=0;
hare 0:6f7d125a0503 156 }