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@9:b58e7d72a91c, 2017-10-02 (annotated)

Committer:
8mona
Date:
Mon Oct 02 19:12:02 2017 +0000
Revision:
9:b58e7d72a91c
Parent:
8:f7ad1d7176ba
Child:
10:ef379cbc0004
Enable non-sensor mode and define curve

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 9:b58e7d72a91c 8 //float vol_accel_ini[ACCEL_SIZE] = {0,0.2,0.8,1,1,1};
8mona 9:b58e7d72a91c 9 float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,1.0,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 9:b58e7d72a91c 72
8mona 9:b58e7d72a91c 73 if (bflag_up==1)
8mona 9:b58e7d72a91c 74 {
8mona 9:b58e7d72a91c 75 if (flag_up_stopping==0)
8mona 9:b58e7d72a91c 76 {
8mona 9:b58e7d72a91c 77 if (accel_count<(ACCEL_SIZE-1))
8mona 9:b58e7d72a91c 78 {
8mona 9:b58e7d72a91c 79 speed = vol_accel[accel_count];
8mona 9:b58e7d72a91c 80 accel_count++;
8mona 9:b58e7d72a91c 81 }
8mona 9:b58e7d72a91c 82 else
8mona 9:b58e7d72a91c 83 {
8mona 9:b58e7d72a91c 84 speed = vol_accel[ACCEL_SIZE-1];
8mona 9:b58e7d72a91c 85 }
8mona 9:b58e7d72a91c 86 }
8mona 9:b58e7d72a91c 87 else if(flag_up_stopping==1)
8mona 9:b58e7d72a91c 88 {
8mona 9:b58e7d72a91c 89 if (decel_count<=(DECEL_SIZE-1))
8mona 9:b58e7d72a91c 90 {
8mona 9:b58e7d72a91c 91 speed = vol_decel[decel_count];
8mona 9:b58e7d72a91c 92 decel_count++;
8mona 9:b58e7d72a91c 93 }
8mona 9:b58e7d72a91c 94 else
8mona 9:b58e7d72a91c 95 {
8mona 9:b58e7d72a91c 96 bflag_up=0;
8mona 9:b58e7d72a91c 97 speed=0;
8mona 9:b58e7d72a91c 98 flag_up_stopping=0;
8mona 9:b58e7d72a91c 99 }
8mona 8:f7ad1d7176ba 100 }
8mona 8:f7ad1d7176ba 101 }
8mona 9:b58e7d72a91c 102 else if (bflag_down==1)
8mona 9:b58e7d72a91c 103 {
8mona 9:b58e7d72a91c 104 if (flag_down_stopping==0)
8mona 9:b58e7d72a91c 105 {
8mona 9:b58e7d72a91c 106 if (accel_count<(ACCEL_SIZE-1))
8mona 9:b58e7d72a91c 107 {
8mona 9:b58e7d72a91c 108 speed = vol_accel[accel_count];
8mona 9:b58e7d72a91c 109 accel_count++;
8mona 9:b58e7d72a91c 110 }
8mona 9:b58e7d72a91c 111 else
8mona 9:b58e7d72a91c 112 {
8mona 9:b58e7d72a91c 113 speed = vol_accel[ACCEL_SIZE-1];
8mona 9:b58e7d72a91c 114 }
8mona 9:b58e7d72a91c 115 }
8mona 9:b58e7d72a91c 116 else if(flag_down_stopping==1)
8mona 9:b58e7d72a91c 117 {
8mona 9:b58e7d72a91c 118 if (decel_count<=(DECEL_SIZE-1))
8mona 9:b58e7d72a91c 119 {
8mona 9:b58e7d72a91c 120 speed = vol_decel[decel_count];
8mona 9:b58e7d72a91c 121 decel_count++;
8mona 9:b58e7d72a91c 122 }
8mona 9:b58e7d72a91c 123 else
8mona 9:b58e7d72a91c 124 {
8mona 9:b58e7d72a91c 125 bflag_down=0;
8mona 9:b58e7d72a91c 126 speed=0;
8mona 9:b58e7d72a91c 127 flag_down_stopping=0;
8mona 9:b58e7d72a91c 128 }
8mona 9:b58e7d72a91c 129 }
8mona 9:b58e7d72a91c 130 }
8mona 9:b58e7d72a91c 131 else
8mona 8:f7ad1d7176ba 132 {
8mona 9:b58e7d72a91c 133 speed=0;
8mona 9:b58e7d72a91c 134 }
8mona 9:b58e7d72a91c 135
8mona 8:f7ad1d7176ba 136
8mona 9:b58e7d72a91c 137 //move motor before push stopping switch
8mona 9:b58e7d72a91c 138 return speed;
8mona 9:b58e7d72a91c 139
8mona 8:f7ad1d7176ba 140
8mona 8:f7ad1d7176ba 141 };
8mona 8:f7ad1d7176ba 142
8mona 8:f7ad1d7176ba 143
8mona 8:f7ad1d7176ba 144