Thinkbed / Mbed 2 deprecated Nucleo_ACM1602_I2C_DC_Angle

Use IQS62X sensor and move motor by detected angle

Dependencies:   DRV8830 IQS62x IQSDisplayTerminal UIT_ACM1602NI mbed

Fork of Nucleo_ACM1602_I2C_DC by Thinkbed

MotorMove.cpp@12:8464be95bf76, 2018-01-30 (annotated)

Committer:
8mona
Date:
Tue Jan 30 06:55:27 2018 +0000
Revision:
12:8464be95bf76
Parent:
11:80b6c5d77073 
Up version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
8mona 8:f7ad1d7176ba 1 #include "mbed.h"
8mona 8:f7ad1d7176ba 2 #include "DRV8830.h"
8mona 8:f7ad1d7176ba 3 #include "MotorMove.h"
8mona 8:f7ad1d7176ba 4 #define SPEED_RATIO 1.0
8mona 8:f7ad1d7176ba 5
8mona 8:f7ad1d7176ba 6 //float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0};
8mona 8:f7ad1d7176ba 7 float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0};
8mona 12:8464be95bf76 8 float vol_accel_ini[ACCEL_SIZE] = {1,1,1,1,1,1};
8mona 12:8464be95bf76 9 //float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,.9,1.0};
8mona 8:f7ad1d7176ba 10
8mona 9:b58e7d72a91c 11 void MotorMove::up_motor_set(int time_counter, float speed)
8mona 8:f7ad1d7176ba 12 {
8mona 9:b58e7d72a91c 13 bflag_up =1;
8mona 8:f7ad1d7176ba 14 bflag_down =0;
8mona 8:f7ad1d7176ba 15 start_time_count=time_counter;
8mona 9:b58e7d72a91c 16 accel_count = 0;
8mona 9:b58e7d72a91c 17 decel_count = 0;
8mona 8:f7ad1d7176ba 18
8mona 8:f7ad1d7176ba 19 for (int i=0;i<ACCEL_SIZE;i++)
8mona 8:f7ad1d7176ba 20 {
8mona 9:b58e7d72a91c 21 vol_accel[i] = -vol_accel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 22 }
8mona 9:b58e7d72a91c 23
8mona 8:f7ad1d7176ba 24 for (int i=0;i<DECEL_SIZE;i++)
8mona 8:f7ad1d7176ba 25 {
8mona 9:b58e7d72a91c 26 vol_decel[i] = -vol_decel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 27 }
8mona 8:f7ad1d7176ba 28 };
8mona 8:f7ad1d7176ba 29
8mona 9:b58e7d72a91c 30 void MotorMove::down_motor_set(int time_counter, float speed)
8mona 8:f7ad1d7176ba 31 {
8mona 9:b58e7d72a91c 32 bflag_up =0;
8mona 9:b58e7d72a91c 33 bflag_down =1;
8mona 8:f7ad1d7176ba 34 start_time_count=time_counter;
8mona 9:b58e7d72a91c 35 accel_count = 0;
8mona 9:b58e7d72a91c 36 decel_count = 0;
8mona 9:b58e7d72a91c 37
8mona 8:f7ad1d7176ba 38 for (int i=0;i<ACCEL_SIZE;i++)
8mona 8:f7ad1d7176ba 39 {
8mona 8:f7ad1d7176ba 40 vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 41 }
8mona 8:f7ad1d7176ba 42
8mona 8:f7ad1d7176ba 43 for (int i=0;i<DECEL_SIZE;i++)
8mona 8:f7ad1d7176ba 44 {
8mona 8:f7ad1d7176ba 45 vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 46 }
8mona 8:f7ad1d7176ba 47 };
8mona 8:f7ad1d7176ba 48
8mona 8:f7ad1d7176ba 49
8mona 9:b58e7d72a91c 50 float MotorMove::ReturnMotorVol(int time_counter,int bflag_downed, int bflag_upped)
8mona 8:f7ad1d7176ba 51 {
8mona 8:f7ad1d7176ba 52 float speed;
8mona 9:b58e7d72a91c 53 int time;
8mona 8:f7ad1d7176ba 54
8mona 9:b58e7d72a91c 55 static int flag_up_stopping=0;
8mona 9:b58e7d72a91c 56 static int flag_down_stopping=0;
8mona 8:f7ad1d7176ba 57
8mona 9:b58e7d72a91c 58 //Check flag for normal mode or brake mode
8mona 9:b58e7d72a91c 59 //accel_count MODE
8mona 9:b58e7d72a91c 60
8mona 8:f7ad1d7176ba 61
8mona 9:b58e7d72a91c 62 if (bflag_up==1 && bflag_upped ==1)
8mona 9:b58e7d72a91c 63 {
8mona 9:b58e7d72a91c 64 flag_up_stopping=1;
8mona 9:b58e7d72a91c 65 //bflag_up==0;
8mona 9:b58e7d72a91c 66 }
8mona 9:b58e7d72a91c 67 else if (bflag_down==1 && bflag_downed ==1)
8mona 9:b58e7d72a91c 68 {
8mona 9:b58e7d72a91c 69 flag_down_stopping=1;
8mona 9:b58e7d72a91c 70 //bflag_down=0;
8mona 9:b58e7d72a91c 71 }
8mona 10:ef379cbc0004 72
8mona 10:ef379cbc0004 73
8mona 10:ef379cbc0004 74 if (bflag_up==1 && bflag_upped==0)
8mona 10:ef379cbc0004 75 {
8mona 10:ef379cbc0004 76 if (accel_count<(ACCEL_SIZE-1))
8mona 10:ef379cbc0004 77 {
8mona 10:ef379cbc0004 78 speed = vol_accel[accel_count];
8mona 10:ef379cbc0004 79 accel_count++;
8mona 10:ef379cbc0004 80 }
8mona 10:ef379cbc0004 81 else
8mona 10:ef379cbc0004 82 {
8mona 10:ef379cbc0004 83 speed = vol_accel[ACCEL_SIZE-1];
8mona 10:ef379cbc0004 84 }
8mona 10:ef379cbc0004 85 }
8mona 10:ef379cbc0004 86 else if(bflag_up==1 && bflag_upped==1)
8mona 10:ef379cbc0004 87 {
8mona 10:ef379cbc0004 88 speed=vol_decel[0];
8mona 10:ef379cbc0004 89 bflag_up=0;
8mona 10:ef379cbc0004 90 }
8mona 10:ef379cbc0004 91 else if (bflag_down==1 && bflag_downed==0)
8mona 10:ef379cbc0004 92 {
8mona 10:ef379cbc0004 93 if (accel_count<(ACCEL_SIZE-1))
8mona 10:ef379cbc0004 94 {
8mona 10:ef379cbc0004 95 speed = vol_accel[accel_count];
8mona 10:ef379cbc0004 96 accel_count++;
8mona 10:ef379cbc0004 97 }
8mona 10:ef379cbc0004 98 else
8mona 10:ef379cbc0004 99 {
8mona 10:ef379cbc0004 100 speed = vol_accel[ACCEL_SIZE-1];
8mona 10:ef379cbc0004 101 }
8mona 10:ef379cbc0004 102 }
8mona 10:ef379cbc0004 103 else if(bflag_down==1 && bflag_downed==1)
8mona 10:ef379cbc0004 104 {
8mona 10:ef379cbc0004 105 speed=vol_decel[0];
8mona 10:ef379cbc0004 106 bflag_down=0;
8mona 10:ef379cbc0004 107 }
8mona 10:ef379cbc0004 108
8mona 10:ef379cbc0004 109
8mona 10:ef379cbc0004 110
8mona 10:ef379cbc0004 111 else
8mona 10:ef379cbc0004 112 {
8mona 10:ef379cbc0004 113 speed=0;
8mona 10:ef379cbc0004 114 }
8mona 10:ef379cbc0004 115
8mona 10:ef379cbc0004 116 /*
8mona 9:b58e7d72a91c 117 if (bflag_up==1)
8mona 9:b58e7d72a91c 118 {
8mona 9:b58e7d72a91c 119 if (flag_up_stopping==0)
8mona 9:b58e7d72a91c 120 {
8mona 9:b58e7d72a91c 121 if (accel_count<(ACCEL_SIZE-1))
8mona 9:b58e7d72a91c 122 {
8mona 9:b58e7d72a91c 123 speed = vol_accel[accel_count];
8mona 9:b58e7d72a91c 124 accel_count++;
8mona 9:b58e7d72a91c 125 }
8mona 9:b58e7d72a91c 126 else
8mona 9:b58e7d72a91c 127 {
8mona 9:b58e7d72a91c 128 speed = vol_accel[ACCEL_SIZE-1];
8mona 9:b58e7d72a91c 129 }
8mona 9:b58e7d72a91c 130 }
8mona 9:b58e7d72a91c 131 else if(flag_up_stopping==1)
8mona 9:b58e7d72a91c 132 {
8mona 9:b58e7d72a91c 133 if (decel_count<=(DECEL_SIZE-1))
8mona 9:b58e7d72a91c 134 {
8mona 9:b58e7d72a91c 135 speed = vol_decel[decel_count];
8mona 9:b58e7d72a91c 136 decel_count++;
8mona 9:b58e7d72a91c 137 }
8mona 9:b58e7d72a91c 138 else
8mona 9:b58e7d72a91c 139 {
8mona 9:b58e7d72a91c 140 bflag_up=0;
8mona 9:b58e7d72a91c 141 speed=0;
8mona 9:b58e7d72a91c 142 flag_up_stopping=0;
8mona 9:b58e7d72a91c 143 }
8mona 8:f7ad1d7176ba 144 }
8mona 8:f7ad1d7176ba 145 }
8mona 9:b58e7d72a91c 146 else if (bflag_down==1)
8mona 9:b58e7d72a91c 147 {
8mona 9:b58e7d72a91c 148 if (flag_down_stopping==0)
8mona 9:b58e7d72a91c 149 {
8mona 9:b58e7d72a91c 150 if (accel_count<(ACCEL_SIZE-1))
8mona 9:b58e7d72a91c 151 {
8mona 9:b58e7d72a91c 152 speed = vol_accel[accel_count];
8mona 9:b58e7d72a91c 153 accel_count++;
8mona 9:b58e7d72a91c 154 }
8mona 9:b58e7d72a91c 155 else
8mona 9:b58e7d72a91c 156 {
8mona 9:b58e7d72a91c 157 speed = vol_accel[ACCEL_SIZE-1];
8mona 9:b58e7d72a91c 158 }
8mona 9:b58e7d72a91c 159 }
8mona 9:b58e7d72a91c 160 else if(flag_down_stopping==1)
8mona 9:b58e7d72a91c 161 {
8mona 9:b58e7d72a91c 162 if (decel_count<=(DECEL_SIZE-1))
8mona 9:b58e7d72a91c 163 {
8mona 9:b58e7d72a91c 164 speed = vol_decel[decel_count];
8mona 9:b58e7d72a91c 165 decel_count++;
8mona 9:b58e7d72a91c 166 }
8mona 9:b58e7d72a91c 167 else
8mona 9:b58e7d72a91c 168 {
8mona 9:b58e7d72a91c 169 bflag_down=0;
8mona 9:b58e7d72a91c 170 speed=0;
8mona 9:b58e7d72a91c 171 flag_down_stopping=0;
8mona 9:b58e7d72a91c 172 }
8mona 9:b58e7d72a91c 173 }
8mona 9:b58e7d72a91c 174 }
8mona 9:b58e7d72a91c 175 else
8mona 8:f7ad1d7176ba 176 {
8mona 9:b58e7d72a91c 177 speed=0;
8mona 9:b58e7d72a91c 178 }
8mona 10:ef379cbc0004 179 */
8mona 9:b58e7d72a91c 180
8mona 8:f7ad1d7176ba 181
8mona 9:b58e7d72a91c 182 //move motor before push stopping switch
8mona 9:b58e7d72a91c 183 return speed;
8mona 9:b58e7d72a91c 184
8mona 8:f7ad1d7176ba 185
8mona 8:f7ad1d7176ba 186 };
8mona 8:f7ad1d7176ba 187
8mona 8:f7ad1d7176ba 188
8mona 8:f7ad1d7176ba 189