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@8:f7ad1d7176ba, 2017-09-30 (annotated)

Committer:
8mona
Date:
Sat Sep 30 16:43:51 2017 +0000
Revision:
8:f7ad1d7176ba
Child:
9:b58e7d72a91c
Activate IQS62X and motor and IO Switch

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 8:f7ad1d7176ba 8 float vol_accel_ini[ACCEL_SIZE] = {0,0.5,0.8,1,1,1};
8mona 8:f7ad1d7176ba 9
8mona 8:f7ad1d7176ba 10 void MotorMove::up_motor_set(int time_counter, int speed)
8mona 8:f7ad1d7176ba 11 {
8mona 8:f7ad1d7176ba 12 bflag_up =1;
8mona 8:f7ad1d7176ba 13 bflag_down =0;
8mona 8:f7ad1d7176ba 14 start_time_count=time_counter;
8mona 8:f7ad1d7176ba 15
8mona 8:f7ad1d7176ba 16 for (int i=0;i<ACCEL_SIZE;i++)
8mona 8:f7ad1d7176ba 17 {
8mona 8:f7ad1d7176ba 18 vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 19 }
8mona 8:f7ad1d7176ba 20
8mona 8:f7ad1d7176ba 21 for (int i=0;i<DECEL_SIZE;i++)
8mona 8:f7ad1d7176ba 22 {
8mona 8:f7ad1d7176ba 23 vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 24 }
8mona 8:f7ad1d7176ba 25 };
8mona 8:f7ad1d7176ba 26
8mona 8:f7ad1d7176ba 27
8mona 8:f7ad1d7176ba 28 void MotorMove::down_motor_set(int time_counter, int speed)
8mona 8:f7ad1d7176ba 29 {
8mona 8:f7ad1d7176ba 30 bflag_up =0;
8mona 8:f7ad1d7176ba 31 bflag_down=1;
8mona 8:f7ad1d7176ba 32 start_time_count=time_counter;
8mona 8:f7ad1d7176ba 33
8mona 8:f7ad1d7176ba 34 for (int i=0;i<ACCEL_SIZE;i++)
8mona 8:f7ad1d7176ba 35 {
8mona 8:f7ad1d7176ba 36 vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 37 }
8mona 8:f7ad1d7176ba 38
8mona 8:f7ad1d7176ba 39 for (int i=0;i<DECEL_SIZE;i++)
8mona 8:f7ad1d7176ba 40 {
8mona 8:f7ad1d7176ba 41 vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
8mona 8:f7ad1d7176ba 42 }
8mona 8:f7ad1d7176ba 43 };
8mona 8:f7ad1d7176ba 44
8mona 8:f7ad1d7176ba 45
8mona 8:f7ad1d7176ba 46 float MotorMove::ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2)
8mona 8:f7ad1d7176ba 47 {
8mona 8:f7ad1d7176ba 48 float speed;
8mona 8:f7ad1d7176ba 49 int time = time_counter - start_time_count;
8mona 8:f7ad1d7176ba 50
8mona 8:f7ad1d7176ba 51
8mona 8:f7ad1d7176ba 52 static int sw_flag1_pre=0;
8mona 8:f7ad1d7176ba 53 static int sw_flag2_pre=0;
8mona 8:f7ad1d7176ba 54 static int stop_time=0;
8mona 8:f7ad1d7176ba 55
8mona 8:f7ad1d7176ba 56 static int up_decel_count=0;
8mona 8:f7ad1d7176ba 57 static int down_decel_count=0;
8mona 8:f7ad1d7176ba 58
8mona 8:f7ad1d7176ba 59 //decel timer check
8mona 8:f7ad1d7176ba 60 if(bflag_up==1)
8mona 8:f7ad1d7176ba 61 up_decel_count==0;
8mona 8:f7ad1d7176ba 62 if(bflag_down==1)
8mona 8:f7ad1d7176ba 63 down_decel_count==0;
8mona 8:f7ad1d7176ba 64
8mona 8:f7ad1d7176ba 65
8mona 8:f7ad1d7176ba 66
8mona 8:f7ad1d7176ba 67 if(bflag_up)
8mona 8:f7ad1d7176ba 68 {
8mona 8:f7ad1d7176ba 69 if (sw_flag2==1 && sw_flag2_pre==0) //State changed
8mona 8:f7ad1d7176ba 70 {
8mona 8:f7ad1d7176ba 71 stop_time=time_counter;
8mona 8:f7ad1d7176ba 72 }
8mona 8:f7ad1d7176ba 73
8mona 8:f7ad1d7176ba 74 if(sw_flag2==0) // move motor
8mona 8:f7ad1d7176ba 75 {
8mona 8:f7ad1d7176ba 76 if(time>ACCEL_SIZE)
8mona 8:f7ad1d7176ba 77 {
8mona 8:f7ad1d7176ba 78 time = ACCEL_SIZE;
8mona 8:f7ad1d7176ba 79 }
8mona 8:f7ad1d7176ba 80 speed = vol_accel[time];
8mona 8:f7ad1d7176ba 81 }
8mona 8:f7ad1d7176ba 82
8mona 8:f7ad1d7176ba 83 else if (sw_flag2==1)
8mona 8:f7ad1d7176ba 84 {
8mona 8:f7ad1d7176ba 85 time = time_counter-stop_time;
8mona 8:f7ad1d7176ba 86
8mona 8:f7ad1d7176ba 87 if(time>DECEL_SIZE)
8mona 8:f7ad1d7176ba 88 {
8mona 8:f7ad1d7176ba 89 time = DECEL_SIZE;
8mona 8:f7ad1d7176ba 90 bflag_up =0;
8mona 8:f7ad1d7176ba 91 }
8mona 8:f7ad1d7176ba 92 speed = vol_decel[time];
8mona 8:f7ad1d7176ba 93 }
8mona 8:f7ad1d7176ba 94 }
8mona 8:f7ad1d7176ba 95
8mona 8:f7ad1d7176ba 96 else if (bflag_down)
8mona 8:f7ad1d7176ba 97 {
8mona 8:f7ad1d7176ba 98 if (sw_flag1==1 && sw_flag1_pre==0) //State changed
8mona 8:f7ad1d7176ba 99 {
8mona 8:f7ad1d7176ba 100 stop_time=time_counter;
8mona 8:f7ad1d7176ba 101 }
8mona 8:f7ad1d7176ba 102
8mona 8:f7ad1d7176ba 103 if(sw_flag1==0) // move motor
8mona 8:f7ad1d7176ba 104 {
8mona 8:f7ad1d7176ba 105 if(time>ACCEL_SIZE)
8mona 8:f7ad1d7176ba 106 {
8mona 8:f7ad1d7176ba 107 time = ACCEL_SIZE;
8mona 8:f7ad1d7176ba 108 }
8mona 8:f7ad1d7176ba 109 speed = -vol_accel[time];
8mona 8:f7ad1d7176ba 110 }
8mona 8:f7ad1d7176ba 111
8mona 8:f7ad1d7176ba 112 else if (sw_flag1==1)
8mona 8:f7ad1d7176ba 113 {
8mona 8:f7ad1d7176ba 114 time = time_counter-stop_time;
8mona 8:f7ad1d7176ba 115 if(time>DECEL_SIZE)
8mona 8:f7ad1d7176ba 116 {
8mona 8:f7ad1d7176ba 117 time = DECEL_SIZE;
8mona 8:f7ad1d7176ba 118 bflag_down =0;
8mona 8:f7ad1d7176ba 119 }
8mona 8:f7ad1d7176ba 120 speed = -vol_decel[time];
8mona 8:f7ad1d7176ba 121 }
8mona 8:f7ad1d7176ba 122 }
8mona 8:f7ad1d7176ba 123
8mona 8:f7ad1d7176ba 124 sw_flag1_pre=sw_flag1;
8mona 8:f7ad1d7176ba 125 sw_flag2_pre=sw_flag2;
8mona 8:f7ad1d7176ba 126 return speed;
8mona 8:f7ad1d7176ba 127 };
8mona 8:f7ad1d7176ba 128
8mona 8:f7ad1d7176ba 129
8mona 8:f7ad1d7176ba 130