frederic blanc / Mbed 2 deprecated qei_hw_motor

Keil MBC1700 demoboard and L293, QEI ,TFT, MOTOR, UART

Dependencies:   mbed Motor

main.cpp@0:9555690e7705, 2012-03-02 (annotated)

Committer:
fblanc
Date:
Fri Mar 02 14:18:23 2012 +0000
Revision:
0:9555690e7705
v1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fblanc 0:9555690e7705 1 /**
fblanc 0:9555690e7705 2 * @file qei_hw_motor
fblanc 0:9555690e7705 3 * @brief Keil MBC1700 demoboard and L293, QEI ,TFT, MOTOR, UART
fblanc 0:9555690e7705 4 * C27 and C81 removed
fblanc 0:9555690e7705 5 * @author Frederic BLANC (Published 01/03/2012 www.mbed.org)
fblanc 0:9555690e7705 6 */
fblanc 0:9555690e7705 7 #include "mbed.h"
fblanc 0:9555690e7705 8 #include "qeihw.h"
fblanc 0:9555690e7705 9 #include "GLCD_MCB1700.h"
fblanc 0:9555690e7705 10 #include "Motor.h"
fblanc 0:9555690e7705 11 #define PPR 624 //Pulse per round of encoder
fblanc 0:9555690e7705 12 #define PWM 0.7 // 0 to 1.0
fblanc 0:9555690e7705 13 //#define TFT 1 //if comment TFT is OFF else if define TFT is ON (µvision limit 32Ko)
fblanc 0:9555690e7705 14 //LED
fblanc 0:9555690e7705 15 DigitalOut led_DIR(P1_28);
fblanc 0:9555690e7705 16 DigitalOut led_POS0(P1_29);
fblanc 0:9555690e7705 17 DigitalOut led_POS1(P1_31);
fblanc 0:9555690e7705 18 DigitalOut led_POS2(P2_2);
fblanc 0:9555690e7705 19 //reseved MOTOR P2_3, P2_4, P2_5
fblanc 0:9555690e7705 20 DigitalOut led_RX(P2_6);
fblanc 0:9555690e7705 21
fblanc 0:9555690e7705 22
fblanc 0:9555690e7705 23 //QEI
fblanc 0:9555690e7705 24 QEIHW qei(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE );
fblanc 0:9555690e7705 25 //TFT
fblanc 0:9555690e7705 26 char text[32];
fblanc 0:9555690e7705 27 // MOTOR
fblanc 0:9555690e7705 28 Motor m(P2_3, P2_4, P2_5); // pwm, fwd, rev L293
fblanc 0:9555690e7705 29 int consigne=0;
fblanc 0:9555690e7705 30 float pwm=0;
fblanc 0:9555690e7705 31 int sens=0;
fblanc 0:9555690e7705 32 //serial COM0
fblanc 0:9555690e7705 33 Serial pc(P0_2, P0_3);//Tx Rx
fblanc 0:9555690e7705 34 void pc_rx(void) {
fblanc 0:9555690e7705 35 char c;
fblanc 0:9555690e7705 36 led_RX=1;
fblanc 0:9555690e7705 37 c=pc.getc();
fblanc 0:9555690e7705 38 switch (c) {
fblanc 0:9555690e7705 39 // consigne
fblanc 0:9555690e7705 40 case '0':
fblanc 0:9555690e7705 41 case '1':
fblanc 0:9555690e7705 42 case '2':
fblanc 0:9555690e7705 43 case '3':
fblanc 0:9555690e7705 44 case '4':
fblanc 0:9555690e7705 45 case '5':
fblanc 0:9555690e7705 46 case '6':
fblanc 0:9555690e7705 47 case '7':
fblanc 0:9555690e7705 48 case '8':
fblanc 0:9555690e7705 49 case '9':
fblanc 0:9555690e7705 50 pc.printf("\r\nconsigne=");
fblanc 0:9555690e7705 51 char buf[256];
fblanc 0:9555690e7705 52 buf[0]=c;
fblanc 0:9555690e7705 53 pc.putc(c);
fblanc 0:9555690e7705 54 for (int i=1; c!='\r'; ++i) {
fblanc 0:9555690e7705 55 c=pc.getc();
fblanc 0:9555690e7705 56 buf[i]=c;
fblanc 0:9555690e7705 57 pc.putc(c);
fblanc 0:9555690e7705 58
fblanc 0:9555690e7705 59 }
fblanc 0:9555690e7705 60 pc.putc('\n');
fblanc 0:9555690e7705 61 consigne=atoi(buf);
fblanc 0:9555690e7705 62
fblanc 0:9555690e7705 63 case '+':
fblanc 0:9555690e7705 64 if (c=='+') {
fblanc 0:9555690e7705 65 ++consigne;
fblanc 0:9555690e7705 66 pc.putc(c);
fblanc 0:9555690e7705 67 }
fblanc 0:9555690e7705 68 case '-':
fblanc 0:9555690e7705 69 if (c=='-') {
fblanc 0:9555690e7705 70 --consigne;
fblanc 0:9555690e7705 71 pc.putc(c);
fblanc 0:9555690e7705 72 }
fblanc 0:9555690e7705 73
fblanc 0:9555690e7705 74 if (consigne<100)
fblanc 0:9555690e7705 75 consigne=100;
fblanc 0:9555690e7705 76 if (consigne>600)
fblanc 0:9555690e7705 77 consigne=600;
fblanc 0:9555690e7705 78 qei.SetPositionComp(QEI_COMPPOS_CH_0,consigne);
fblanc 0:9555690e7705 79 qei.IntCmd(QEI_INTFLAG_POS0_Int, ENABLE);
fblanc 0:9555690e7705 80 if (consigne!=0) {
fblanc 0:9555690e7705 81 qei.SetPositionComp(QEI_COMPPOS_CH_1,consigne-1);
fblanc 0:9555690e7705 82 qei.IntCmd(QEI_INTFLAG_POS1_Int, ENABLE);
fblanc 0:9555690e7705 83 }
fblanc 0:9555690e7705 84 qei.SetPositionComp(QEI_COMPPOS_CH_2,consigne+1);
fblanc 0:9555690e7705 85 qei.IntCmd(QEI_INTFLAG_POS2_Int, ENABLE);
fblanc 0:9555690e7705 86 int tmp;
fblanc 0:9555690e7705 87 tmp = consigne-qei.GetPosition();
fblanc 0:9555690e7705 88 if (tmp > 0)
fblanc 0:9555690e7705 89 sens=1;
fblanc 0:9555690e7705 90 else if (tmp < 0)
fblanc 0:9555690e7705 91 sens=-1;
fblanc 0:9555690e7705 92 else if (tmp ==0)
fblanc 0:9555690e7705 93 sens=0;
fblanc 0:9555690e7705 94
fblanc 0:9555690e7705 95 break;
fblanc 0:9555690e7705 96 //PWM
fblanc 0:9555690e7705 97 case 'P':
fblanc 0:9555690e7705 98 case 'p':
fblanc 0:9555690e7705 99 pc.printf("\r\nPWM=");
fblanc 0:9555690e7705 100 pc.scanf("%f",&pwm);
fblanc 0:9555690e7705 101 pc.printf("%f\r\n",pwm);
fblanc 0:9555690e7705 102
fblanc 0:9555690e7705 103 break;
fblanc 0:9555690e7705 104 //STOP
fblanc 0:9555690e7705 105 case ' ':
fblanc 0:9555690e7705 106 pc.printf("\r\nSTOP\r\n");
fblanc 0:9555690e7705 107 sens=0;
fblanc 0:9555690e7705 108 break;
fblanc 0:9555690e7705 109
fblanc 0:9555690e7705 110 }
fblanc 0:9555690e7705 111 led_RX=0;
fblanc 0:9555690e7705 112 }
fblanc 0:9555690e7705 113 //ISR
fblanc 0:9555690e7705 114 void isr_POS0(void) {
fblanc 0:9555690e7705 115 led_POS0 = 1;
fblanc 0:9555690e7705 116 sens=0;
fblanc 0:9555690e7705 117 qei.IntClear(QEI_INTFLAG_POS0_Int);
fblanc 0:9555690e7705 118
fblanc 0:9555690e7705 119 }
fblanc 0:9555690e7705 120
fblanc 0:9555690e7705 121 void isr_POS1(void) {
fblanc 0:9555690e7705 122 led_POS1 = 1;
fblanc 0:9555690e7705 123 sens=1;
fblanc 0:9555690e7705 124 qei.IntClear(QEI_INTFLAG_POS1_Int);
fblanc 0:9555690e7705 125 }
fblanc 0:9555690e7705 126
fblanc 0:9555690e7705 127 void isr_POS2(void) {
fblanc 0:9555690e7705 128 led_POS2 = 1;
fblanc 0:9555690e7705 129 sens=-1;
fblanc 0:9555690e7705 130 qei.IntClear(QEI_INTFLAG_POS2_Int);
fblanc 0:9555690e7705 131 }
fblanc 0:9555690e7705 132
fblanc 0:9555690e7705 133 int main() {
fblanc 0:9555690e7705 134 //init GLCD
fblanc 0:9555690e7705 135 #ifdef TFT
fblanc 0:9555690e7705 136 GLCD_Init();
fblanc 0:9555690e7705 137 GLCD_Clear (White);
fblanc 0:9555690e7705 138 sprintf(text,"QEI HARDWARE MOTOR");
fblanc 0:9555690e7705 139 GLCD_DisplayString (0, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 140 #endif
fblanc 0:9555690e7705 141 //init QEI
fblanc 0:9555690e7705 142 qei.SetDigiFilter(1UL); //4294967295
fblanc 0:9555690e7705 143 qei.SetMaxPosition(0xFFFFFFFF);
fblanc 0:9555690e7705 144 qei.SetVelocityTimerReload_us(100000);
fblanc 0:9555690e7705 145 //isr POS0
fblanc 0:9555690e7705 146 qei.AppendISR(QEI_INTFLAG_POS0_Int, &isr_POS0);
fblanc 0:9555690e7705 147 qei.AppendISR(QEI_INTFLAG_POS1_Int, &isr_POS1);
fblanc 0:9555690e7705 148 qei.AppendISR(QEI_INTFLAG_POS2_Int, &isr_POS2);
fblanc 0:9555690e7705 149
fblanc 0:9555690e7705 150 pwm=PWM;
fblanc 0:9555690e7705 151 while (1) {
fblanc 0:9555690e7705 152 led_POS0 = 0;
fblanc 0:9555690e7705 153 led_POS1 = 0;
fblanc 0:9555690e7705 154 led_POS2 = 0;
fblanc 0:9555690e7705 155 led_RX=0;
fblanc 0:9555690e7705 156 //TFT
fblanc 0:9555690e7705 157 #ifdef TFT
fblanc 0:9555690e7705 158 sprintf(text,"pos : %d vs %d", qei.GetPosition(),consigne);
fblanc 0:9555690e7705 159 GLCD_DisplayString (1, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 160 sprintf(text,"idx : %d ", qei.GetIndex());
fblanc 0:9555690e7705 161 GLCD_DisplayString (2, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 162 sprintf(text,"dir : %d ", qei.Direction() == SET ? -1 : 1);
fblanc 0:9555690e7705 163 GLCD_DisplayString (3, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 164 sprintf(text,"vel : %d ", qei.GetVelocity ());
fblanc 0:9555690e7705 165 GLCD_DisplayString (4, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 166 sprintf(text,"rpm : %d ", qei.CalculateRPM (qei.GetVelocityCap (), PPR));
fblanc 0:9555690e7705 167 GLCD_DisplayString (5, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 168 sprintf(text,"PWM : %f ", pwm);
fblanc 0:9555690e7705 169 GLCD_DisplayString (5, 0, 1, (unsigned char *)text);
fblanc 0:9555690e7705 170 #endif
fblanc 0:9555690e7705 171 led_DIR = qei.Direction() == SET ? 1 : 0;
fblanc 0:9555690e7705 172 m.speed(sens * pwm);
fblanc 0:9555690e7705 173 if (pc.readable())
fblanc 0:9555690e7705 174 pc_rx();
fblanc 0:9555690e7705 175 }
fblanc 0:9555690e7705 176 }