![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ultima versione funzionante
Dependencies: mbed
BLE-MegaPiSea_Rev2_WIP.cpp@2:fec3ed956ded, 2019-06-27 (annotated)
- Committer:
- pinofal
- Date:
- Thu Jun 27 09:08:17 2019 +0000
- Revision:
- 2:fec3ed956ded
- Child:
- 3:b1e7eb9d61b7
Robot Funzionante
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pinofal | 2:fec3ed956ded | 1 | // bpilotaggio carrello tramite BLE. |
pinofal | 2:fec3ed956ded | 2 | // testato su L476RG |
pinofal | 2:fec3ed956ded | 3 | |
pinofal | 2:fec3ed956ded | 4 | #include "mbed.h" |
pinofal | 2:fec3ed956ded | 5 | #include<stdlib.h> |
pinofal | 2:fec3ed956ded | 6 | |
pinofal | 2:fec3ed956ded | 7 | // pi greco |
pinofal | 2:fec3ed956ded | 8 | #define PI 3.14159265358979323846 |
pinofal | 2:fec3ed956ded | 9 | |
pinofal | 2:fec3ed956ded | 10 | // Definizione periferica USB seriale del PC |
pinofal | 2:fec3ed956ded | 11 | Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 |
pinofal | 2:fec3ed956ded | 12 | |
pinofal | 2:fec3ed956ded | 13 | // Definizione periferica seriale del Modulo BLE ELETT114A |
pinofal | 2:fec3ed956ded | 14 | Serial myBLE(PA_9, PA_10, 115200); //Tx, Rx, bps |
pinofal | 2:fec3ed956ded | 15 | |
pinofal | 2:fec3ed956ded | 16 | // Input di Reset per il Modulo BLE ELETT114A. |
pinofal | 2:fec3ed956ded | 17 | DigitalOut BleRst(PA_8); |
pinofal | 2:fec3ed956ded | 18 | |
pinofal | 2:fec3ed956ded | 19 | // User Button, LED |
pinofal | 2:fec3ed956ded | 20 | DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13 |
pinofal | 2:fec3ed956ded | 21 | DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5 |
pinofal | 2:fec3ed956ded | 22 | |
pinofal | 2:fec3ed956ded | 23 | |
pinofal | 2:fec3ed956ded | 24 | |
pinofal | 2:fec3ed956ded | 25 | |
pinofal | 2:fec3ed956ded | 26 | // carattere letto dalla seriale del PC |
pinofal | 2:fec3ed956ded | 27 | char cReadChar; |
pinofal | 2:fec3ed956ded | 28 | |
pinofal | 2:fec3ed956ded | 29 | // DutyCycle del segnale PWM |
pinofal | 2:fec3ed956ded | 30 | float fDutyCycle; |
pinofal | 2:fec3ed956ded | 31 | |
pinofal | 2:fec3ed956ded | 32 | |
pinofal | 2:fec3ed956ded | 33 | // Input/Output |
pinofal | 2:fec3ed956ded | 34 | DigitalOut PostOutBI1 (PA_6); // Output 1 per pilotaggio input BI1 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 35 | PwmOut PostOutPWB (PA_7); // Output per pilotaggio input PWM del motore B Posteriore |
pinofal | 2:fec3ed956ded | 36 | //DigitalOut PostOutPWB (PA_7); // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore |
pinofal | 2:fec3ed956ded | 37 | DigitalOut PostOutBI2 (PB_6); // Output 2 per pilotaggio input BI2 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 38 | DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore |
pinofal | 2:fec3ed956ded | 39 | |
pinofal | 2:fec3ed956ded | 40 | //DigitalInOut OutBlades (PB_9, PIN_OUTPUT, OpenDrain, 0); // Output per il pilotaggio del Relay di azionamento Lame Rotanti |
pinofal | 2:fec3ed956ded | 41 | |
pinofal | 2:fec3ed956ded | 42 | |
pinofal | 2:fec3ed956ded | 43 | DigitalOut AntOutBI1 (PB_4); // Output 1 per pilotaggio input BI1 del Motore B Anteriore |
pinofal | 2:fec3ed956ded | 44 | PwmOut AntOutPWB (PB_5); // Output per pilotaggio input PWM del motore B Anteriore |
pinofal | 2:fec3ed956ded | 45 | //DigitalOut AntOutPWB (PB_5); // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore |
pinofal | 2:fec3ed956ded | 46 | DigitalOut AntOutBI2 (PB_3); // Output 2 per pilotaggio input BI2 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 47 | DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore |
pinofal | 2:fec3ed956ded | 48 | |
pinofal | 2:fec3ed956ded | 49 | |
pinofal | 2:fec3ed956ded | 50 | |
pinofal | 2:fec3ed956ded | 51 | /* |
pinofal | 2:fec3ed956ded | 52 | //+++DigitalIn PostInNE1 (BHO); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore |
pinofal | 2:fec3ed956ded | 53 | DigitalOut PostOutBI2 (PG_15); // Output 2 per pilotaggio input BI2 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 54 | DigitalOut PostOutBI1 (PA_15); // Output 1 per pilotaggio input BI1 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 55 | //PwmOut PostOutPWB (PH_13); // Output per pilotaggio input PWM del motore B Posteriore |
pinofal | 2:fec3ed956ded | 56 | DigitalOut PostOutPWB (PH_13); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore |
pinofal | 2:fec3ed956ded | 57 | |
pinofal | 2:fec3ed956ded | 58 | DigitalOut AntOutBI2 (PB_5); // Output 2 per pilotaggio input BI2 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 59 | DigitalOut AntOutBI1 (PA_5); // Output 1 per pilotaggio input BI1 del Motore B Posteriore |
pinofal | 2:fec3ed956ded | 60 | //PwmOut AntOutPWB (PB_4); // Output per pilotaggio input PWM del motore B Posteriore |
pinofal | 2:fec3ed956ded | 61 | DigitalOut AntOutPWB (PB_4); // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore |
pinofal | 2:fec3ed956ded | 62 | */ |
pinofal | 2:fec3ed956ded | 63 | |
pinofal | 2:fec3ed956ded | 64 | |
pinofal | 2:fec3ed956ded | 65 | //carattere di comando ricevuto dal BLE |
pinofal | 2:fec3ed956ded | 66 | volatile char cCommandBLE; // cambia nella routine di interrupt |
pinofal | 2:fec3ed956ded | 67 | // memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente |
pinofal | 2:fec3ed956ded | 68 | char cOldCommandBLE; |
pinofal | 2:fec3ed956ded | 69 | |
pinofal | 2:fec3ed956ded | 70 | // coordinate polari del joistick sulla APP, fornite dalla routine di interrupt |
pinofal | 2:fec3ed956ded | 71 | volatile double fTeta; |
pinofal | 2:fec3ed956ded | 72 | volatile double fRo; |
pinofal | 2:fec3ed956ded | 73 | |
pinofal | 2:fec3ed956ded | 74 | // coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt |
pinofal | 2:fec3ed956ded | 75 | volatile double fX, fY; |
pinofal | 2:fec3ed956ded | 76 | // memorizza ultimi valori delle coordinate del joistick |
pinofal | 2:fec3ed956ded | 77 | double fOldX, fOldY; |
pinofal | 2:fec3ed956ded | 78 | |
pinofal | 2:fec3ed956ded | 79 | // variabili ausiliarie per l'algoritmo di posizionamento |
pinofal | 2:fec3ed956ded | 80 | double fV, fW; |
pinofal | 2:fec3ed956ded | 81 | |
pinofal | 2:fec3ed956ded | 82 | // velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore |
pinofal | 2:fec3ed956ded | 83 | double fR, fL; |
pinofal | 2:fec3ed956ded | 84 | |
pinofal | 2:fec3ed956ded | 85 | |
pinofal | 2:fec3ed956ded | 86 | |
pinofal | 2:fec3ed956ded | 87 | |
pinofal | 2:fec3ed956ded | 88 | |
pinofal | 2:fec3ed956ded | 89 | //**********************************************/ |
pinofal | 2:fec3ed956ded | 90 | // IRQ associata a Rx da BLE |
pinofal | 2:fec3ed956ded | 91 | //**********************************************/ |
pinofal | 2:fec3ed956ded | 92 | void BLERxInterrupt(void) |
pinofal | 2:fec3ed956ded | 93 | { |
pinofal | 2:fec3ed956ded | 94 | // array per la ricezione dei messaggi da seriale |
pinofal | 2:fec3ed956ded | 95 | char cReadChar; |
pinofal | 2:fec3ed956ded | 96 | |
pinofal | 2:fec3ed956ded | 97 | //indice per i cicli |
pinofal | 2:fec3ed956ded | 98 | int nIndex; |
pinofal | 2:fec3ed956ded | 99 | |
pinofal | 2:fec3ed956ded | 100 | // array per la ricezione dei messaggi da seriale |
pinofal | 2:fec3ed956ded | 101 | char caRxPacket[8]; |
pinofal | 2:fec3ed956ded | 102 | //int nRxPacketSize; |
pinofal | 2:fec3ed956ded | 103 | |
pinofal | 2:fec3ed956ded | 104 | // coordinate cartesiane della posizione joystick |
pinofal | 2:fec3ed956ded | 105 | //float fX, fY; |
pinofal | 2:fec3ed956ded | 106 | // coordinate polari della posizione joistick |
pinofal | 2:fec3ed956ded | 107 | //float fTeta; |
pinofal | 2:fec3ed956ded | 108 | //float fRo; |
pinofal | 2:fec3ed956ded | 109 | |
pinofal | 2:fec3ed956ded | 110 | |
pinofal | 2:fec3ed956ded | 111 | |
pinofal | 2:fec3ed956ded | 112 | //pc.printf("BLE RxInterrupt: \n\r"); |
pinofal | 2:fec3ed956ded | 113 | |
pinofal | 2:fec3ed956ded | 114 | // ricevi caratteri su seriale, se disponibili |
pinofal | 2:fec3ed956ded | 115 | while((myBLE.readable())) |
pinofal | 2:fec3ed956ded | 116 | { |
pinofal | 2:fec3ed956ded | 117 | // acquisice stringa in input e relativa dimensione |
pinofal | 2:fec3ed956ded | 118 | cReadChar = myBLE.getc(); // Read character |
pinofal | 2:fec3ed956ded | 119 | if(cReadChar == 0x02) |
pinofal | 2:fec3ed956ded | 120 | { |
pinofal | 2:fec3ed956ded | 121 | //-- command will be 8 bytes for joystick values |
pinofal | 2:fec3ed956ded | 122 | //-- command will be 3 bytes for button change event |
pinofal | 2:fec3ed956ded | 123 | //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03) |
pinofal | 2:fec3ed956ded | 124 | |
pinofal | 2:fec3ed956ded | 125 | |
pinofal | 2:fec3ed956ded | 126 | caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX |
pinofal | 2:fec3ed956ded | 127 | cReadChar = myBLE.getc(); // legge il secondo carattere |
pinofal | 2:fec3ed956ded | 128 | if(cReadChar > 0x40) |
pinofal | 2:fec3ed956ded | 129 | { |
pinofal | 2:fec3ed956ded | 130 | // Button: |
pinofal | 2:fec3ed956ded | 131 | //-- Button events send a single character in a 3-byte packet |
pinofal | 2:fec3ed956ded | 132 | //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX> |
pinofal | 2:fec3ed956ded | 133 | //-- B2 uses "C" for changed to on, "D" for changed to off |
pinofal | 2:fec3ed956ded | 134 | //-- B3..B6 follow in order; valid button even characters are "A".."L" |
pinofal | 2:fec3ed956ded | 135 | caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP |
pinofal | 2:fec3ed956ded | 136 | caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX |
pinofal | 2:fec3ed956ded | 137 | // passa il comando ricevuto nella variabile globale |
pinofal | 2:fec3ed956ded | 138 | cCommandBLE = caRxPacket[1]; |
pinofal | 2:fec3ed956ded | 139 | |
pinofal | 2:fec3ed956ded | 140 | // Diagnostica |
pinofal | 2:fec3ed956ded | 141 | /* |
pinofal | 2:fec3ed956ded | 142 | pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6 |
pinofal | 2:fec3ed956ded | 143 | */ |
pinofal | 2:fec3ed956ded | 144 | |
pinofal | 2:fec3ed956ded | 145 | } |
pinofal | 2:fec3ed956ded | 146 | else |
pinofal | 2:fec3ed956ded | 147 | { |
pinofal | 2:fec3ed956ded | 148 | // Joystick: |
pinofal | 2:fec3ed956ded | 149 | //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis |
pinofal | 2:fec3ed956ded | 150 | //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator |
pinofal | 2:fec3ed956ded | 151 | //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX> |
pinofal | 2:fec3ed956ded | 152 | caRxPacket[1] = cReadChar; // memorizza il secondo carattere |
pinofal | 2:fec3ed956ded | 153 | for(nIndex=2; nIndex<8; nIndex++) |
pinofal | 2:fec3ed956ded | 154 | { |
pinofal | 2:fec3ed956ded | 155 | caRxPacket[nIndex] = myBLE.getc(); |
pinofal | 2:fec3ed956ded | 156 | } |
pinofal | 2:fec3ed956ded | 157 | // dal messaggio estrae e visualizza le coordinate cartesiane |
pinofal | 2:fec3ed956ded | 158 | fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200); |
pinofal | 2:fec3ed956ded | 159 | fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200); |
pinofal | 2:fec3ed956ded | 160 | // converte la posizione del joistick in coordinate polari |
pinofal | 2:fec3ed956ded | 161 | fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo |
pinofal | 2:fec3ed956ded | 162 | if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360° |
pinofal | 2:fec3ed956ded | 163 | fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100 |
pinofal | 2:fec3ed956ded | 164 | |
pinofal | 2:fec3ed956ded | 165 | // diagnostica |
pinofal | 2:fec3ed956ded | 166 | /* |
pinofal | 2:fec3ed956ded | 167 | pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE |
pinofal | 2:fec3ed956ded | 168 | pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane |
pinofal | 2:fec3ed956ded | 169 | pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari |
pinofal | 2:fec3ed956ded | 170 | */ |
pinofal | 2:fec3ed956ded | 171 | } |
pinofal | 2:fec3ed956ded | 172 | } |
pinofal | 2:fec3ed956ded | 173 | } |
pinofal | 2:fec3ed956ded | 174 | |
pinofal | 2:fec3ed956ded | 175 | } |
pinofal | 2:fec3ed956ded | 176 | |
pinofal | 2:fec3ed956ded | 177 | /**********************************************/ |
pinofal | 2:fec3ed956ded | 178 | // IRQ associata a Rx da PC |
pinofal | 2:fec3ed956ded | 179 | //**********************************************/ |
pinofal | 2:fec3ed956ded | 180 | void pcRxInterrupt(void) |
pinofal | 2:fec3ed956ded | 181 | { |
pinofal | 2:fec3ed956ded | 182 | // array per la ricezione dei messaggi da seriale |
pinofal | 2:fec3ed956ded | 183 | char cReadChar; |
pinofal | 2:fec3ed956ded | 184 | |
pinofal | 2:fec3ed956ded | 185 | // ricevi caratteri su seriale, se disponibili |
pinofal | 2:fec3ed956ded | 186 | while((pc.readable())) |
pinofal | 2:fec3ed956ded | 187 | { |
pinofal | 2:fec3ed956ded | 188 | // acquisice stringa in input e relativa dimensione |
pinofal | 2:fec3ed956ded | 189 | cReadChar = pc.getc(); // read character from PC |
pinofal | 2:fec3ed956ded | 190 | //myBLE.putc(cReadChar); // write char to BLE |
pinofal | 2:fec3ed956ded | 191 | |
pinofal | 2:fec3ed956ded | 192 | //pc.printf("W>: 0x%02x\n\r",cReadChar); |
pinofal | 2:fec3ed956ded | 193 | if(cReadChar == '0') // se scrivo '0', invia questa stringa |
pinofal | 2:fec3ed956ded | 194 | { |
pinofal | 2:fec3ed956ded | 195 | //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX> |
pinofal | 2:fec3ed956ded | 196 | //-- button status is binary formatted string (no indicator) |
pinofal | 2:fec3ed956ded | 197 | //-- data fields sent as strings |
pinofal | 2:fec3ed956ded | 198 | //-- send empty string to unused field (not sure if short response packet is allowed without additional testing) |
pinofal | 2:fec3ed956ded | 199 | // Struttura Nominale del comando da inviare al robot STX , B6, B5, B4, B3 B2 B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX; |
pinofal | 2:fec3ed956ded | 200 | myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03); |
pinofal | 2:fec3ed956ded | 201 | // diagnostica |
pinofal | 2:fec3ed956ded | 202 | /* |
pinofal | 2:fec3ed956ded | 203 | pc.printf("W>: Inviato comando a BLE\n\r"); |
pinofal | 2:fec3ed956ded | 204 | */ |
pinofal | 2:fec3ed956ded | 205 | } |
pinofal | 2:fec3ed956ded | 206 | } |
pinofal | 2:fec3ed956ded | 207 | } |
pinofal | 2:fec3ed956ded | 208 | |
pinofal | 2:fec3ed956ded | 209 | |
pinofal | 2:fec3ed956ded | 210 | /**********/ |
pinofal | 2:fec3ed956ded | 211 | /* MAIN */ |
pinofal | 2:fec3ed956ded | 212 | /**********/ |
pinofal | 2:fec3ed956ded | 213 | int main() |
pinofal | 2:fec3ed956ded | 214 | { |
pinofal | 2:fec3ed956ded | 215 | |
pinofal | 2:fec3ed956ded | 216 | // messaggio di benvenuto |
pinofal | 2:fec3ed956ded | 217 | pc.printf("\r\n******* Hallo - Exercise ********** \r\n"); |
pinofal | 2:fec3ed956ded | 218 | pc.printf("\r\n*** L476 BLE MegaPi Motor Driver ***\r\n"); |
pinofal | 2:fec3ed956ded | 219 | |
pinofal | 2:fec3ed956ded | 220 | //Inizializza pin di output per Rotating Blades |
pinofal | 2:fec3ed956ded | 221 | /* |
pinofal | 2:fec3ed956ded | 222 | OutBlades.mode(OpenDrain); |
pinofal | 2:fec3ed956ded | 223 | OutBlades.output(); |
pinofal | 2:fec3ed956ded | 224 | OutBlades.write(0); |
pinofal | 2:fec3ed956ded | 225 | nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente |
pinofal | 2:fec3ed956ded | 226 | */ |
pinofal | 2:fec3ed956ded | 227 | // inizializza variabili |
pinofal | 2:fec3ed956ded | 228 | cCommandBLE = 0; // inizialmente nessun comando da BLE |
pinofal | 2:fec3ed956ded | 229 | cOldCommandBLE = 0; // inizialmente nessun comando da BLE |
pinofal | 2:fec3ed956ded | 230 | fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 231 | fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 232 | fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 233 | fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 234 | |
pinofal | 2:fec3ed956ded | 235 | // inizializza il BLE |
pinofal | 2:fec3ed956ded | 236 | BleRst = 0; |
pinofal | 2:fec3ed956ded | 237 | wait_ms(100); |
pinofal | 2:fec3ed956ded | 238 | BleRst = 1; |
pinofal | 2:fec3ed956ded | 239 | cCommandBLE = '0'; |
pinofal | 2:fec3ed956ded | 240 | cOldCommandBLE = '0'; |
pinofal | 2:fec3ed956ded | 241 | |
pinofal | 2:fec3ed956ded | 242 | |
pinofal | 2:fec3ed956ded | 243 | |
pinofal | 2:fec3ed956ded | 244 | // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore |
pinofal | 2:fec3ed956ded | 245 | PostOutPWB.period_us(10); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 246 | PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore) |
pinofal | 2:fec3ed956ded | 247 | AntOutPWB.period_us(10); // periodo del PWM Anteriore |
pinofal | 2:fec3ed956ded | 248 | AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore) |
pinofal | 2:fec3ed956ded | 249 | |
pinofal | 2:fec3ed956ded | 250 | |
pinofal | 2:fec3ed956ded | 251 | // Attiva la IRQ per la RX su seriale |
pinofal | 2:fec3ed956ded | 252 | myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE |
pinofal | 2:fec3ed956ded | 253 | pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC |
pinofal | 2:fec3ed956ded | 254 | |
pinofal | 2:fec3ed956ded | 255 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 256 | //++++++++++++++ INIZIO Ciclo Principale +++++++++++++ |
pinofal | 2:fec3ed956ded | 257 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 258 | |
pinofal | 2:fec3ed956ded | 259 | while(true) |
pinofal | 2:fec3ed956ded | 260 | { |
pinofal | 2:fec3ed956ded | 261 | //Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left: |
pinofal | 2:fec3ed956ded | 262 | //Invert X |
pinofal | 2:fec3ed956ded | 263 | //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y |
pinofal | 2:fec3ed956ded | 264 | //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X |
pinofal | 2:fec3ed956ded | 265 | //Calcola R: R = (V+W) /2 |
pinofal | 2:fec3ed956ded | 266 | //Calcola L: L= (V-W)/2 |
pinofal | 2:fec3ed956ded | 267 | //Scala i valori di L e R in base all'hardware. |
pinofal | 2:fec3ed956ded | 268 | //invia i valori al robot. |
pinofal | 2:fec3ed956ded | 269 | |
pinofal | 2:fec3ed956ded | 270 | // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote |
pinofal | 2:fec3ed956ded | 271 | if( (fX != fOldX) || (fY != fOldY)) |
pinofal | 2:fec3ed956ded | 272 | { |
pinofal | 2:fec3ed956ded | 273 | fOldX = fX; |
pinofal | 2:fec3ed956ded | 274 | fOldY = fY; |
pinofal | 2:fec3ed956ded | 275 | // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL) |
pinofal | 2:fec3ed956ded | 276 | fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio |
pinofal | 2:fec3ed956ded | 277 | fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio |
pinofal | 2:fec3ed956ded | 278 | fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100) |
pinofal | 2:fec3ed956ded | 279 | fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100) |
pinofal | 2:fec3ed956ded | 280 | |
pinofal | 2:fec3ed956ded | 281 | // diagnostica |
pinofal | 2:fec3ed956ded | 282 | pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); |
pinofal | 2:fec3ed956ded | 283 | pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); |
pinofal | 2:fec3ed956ded | 284 | pc.printf("> Velocita' Right R = %.2f\r\n", fR); |
pinofal | 2:fec3ed956ded | 285 | pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL); |
pinofal | 2:fec3ed956ded | 286 | |
pinofal | 2:fec3ed956ded | 287 | // algoritmo di movimentazione delle ruote. |
pinofal | 2:fec3ed956ded | 288 | if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore |
pinofal | 2:fec3ed956ded | 289 | { |
pinofal | 2:fec3ed956ded | 290 | fR =-fR; |
pinofal | 2:fec3ed956ded | 291 | // Vai indietro |
pinofal | 2:fec3ed956ded | 292 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 293 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 294 | } |
pinofal | 2:fec3ed956ded | 295 | else |
pinofal | 2:fec3ed956ded | 296 | { |
pinofal | 2:fec3ed956ded | 297 | |
pinofal | 2:fec3ed956ded | 298 | if(fR >0) |
pinofal | 2:fec3ed956ded | 299 | { |
pinofal | 2:fec3ed956ded | 300 | // Vai avanti |
pinofal | 2:fec3ed956ded | 301 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 302 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 303 | } |
pinofal | 2:fec3ed956ded | 304 | else |
pinofal | 2:fec3ed956ded | 305 | { |
pinofal | 2:fec3ed956ded | 306 | // spegni |
pinofal | 2:fec3ed956ded | 307 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 308 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 309 | } |
pinofal | 2:fec3ed956ded | 310 | } |
pinofal | 2:fec3ed956ded | 311 | PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore) |
pinofal | 2:fec3ed956ded | 312 | if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore |
pinofal | 2:fec3ed956ded | 313 | { |
pinofal | 2:fec3ed956ded | 314 | fL =-fL; |
pinofal | 2:fec3ed956ded | 315 | // Vai indietro |
pinofal | 2:fec3ed956ded | 316 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 317 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 318 | } |
pinofal | 2:fec3ed956ded | 319 | else |
pinofal | 2:fec3ed956ded | 320 | { |
pinofal | 2:fec3ed956ded | 321 | if(fL >0) |
pinofal | 2:fec3ed956ded | 322 | { |
pinofal | 2:fec3ed956ded | 323 | // Vai avanti |
pinofal | 2:fec3ed956ded | 324 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 325 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 326 | |
pinofal | 2:fec3ed956ded | 327 | } |
pinofal | 2:fec3ed956ded | 328 | else |
pinofal | 2:fec3ed956ded | 329 | { |
pinofal | 2:fec3ed956ded | 330 | // spegni |
pinofal | 2:fec3ed956ded | 331 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 332 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 333 | } |
pinofal | 2:fec3ed956ded | 334 | } |
pinofal | 2:fec3ed956ded | 335 | AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore) |
pinofal | 2:fec3ed956ded | 336 | } |
pinofal | 2:fec3ed956ded | 337 | } //while (true) Ciclo principale |
pinofal | 2:fec3ed956ded | 338 | |
pinofal | 2:fec3ed956ded | 339 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 340 | //++++++++++++++ FINE Ciclo Principale +++++++++++++ |
pinofal | 2:fec3ed956ded | 341 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 342 | |
pinofal | 2:fec3ed956ded | 343 | |
pinofal | 2:fec3ed956ded | 344 | |
pinofal | 2:fec3ed956ded | 345 | |
pinofal | 2:fec3ed956ded | 346 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 347 | //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 348 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 349 | |
pinofal | 2:fec3ed956ded | 350 | //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++ |
pinofal | 2:fec3ed956ded | 351 | |
pinofal | 2:fec3ed956ded | 352 | // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' |
pinofal | 2:fec3ed956ded | 353 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 354 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 355 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 356 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 357 | |
pinofal | 2:fec3ed956ded | 358 | fDutyCycle = 0.0; |
pinofal | 2:fec3ed956ded | 359 | |
pinofal | 2:fec3ed956ded | 360 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 361 | PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 362 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 363 | AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 2:fec3ed956ded | 364 | AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 365 | while(true) |
pinofal | 2:fec3ed956ded | 366 | { |
pinofal | 2:fec3ed956ded | 367 | //Parsing del comando ricevuto solo se cambia il comando. |
pinofal | 2:fec3ed956ded | 368 | if(cCommandBLE != cOldCommandBLE) |
pinofal | 2:fec3ed956ded | 369 | { |
pinofal | 2:fec3ed956ded | 370 | // ricorda il vecchio comando |
pinofal | 2:fec3ed956ded | 371 | cOldCommandBLE = cCommandBLE; |
pinofal | 2:fec3ed956ded | 372 | //cambia velocità del PWM |
pinofal | 2:fec3ed956ded | 373 | fDutyCycle = double(cOldCommandBLE - 0x41)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5 |
pinofal | 2:fec3ed956ded | 374 | PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 375 | AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 376 | pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle); |
pinofal | 2:fec3ed956ded | 377 | //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM |
pinofal | 2:fec3ed956ded | 378 | //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0)); |
pinofal | 2:fec3ed956ded | 379 | |
pinofal | 2:fec3ed956ded | 380 | //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione |
pinofal | 2:fec3ed956ded | 381 | switch (cOldCommandBLE) // usa cOldCommandBLE. |
pinofal | 2:fec3ed956ded | 382 | { |
pinofal | 2:fec3ed956ded | 383 | case 'A': |
pinofal | 2:fec3ed956ded | 384 | { |
pinofal | 2:fec3ed956ded | 385 | // se ricevi il comando 'A' |
pinofal | 2:fec3ed956ded | 386 | // Vai avanti |
pinofal | 2:fec3ed956ded | 387 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 388 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 389 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 390 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 391 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 392 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 393 | pc.printf("Avanti\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 394 | wait (2); |
pinofal | 2:fec3ed956ded | 395 | |
pinofal | 2:fec3ed956ded | 396 | // spegni |
pinofal | 2:fec3ed956ded | 397 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 398 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 399 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 400 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 401 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 402 | wait (1); |
pinofal | 2:fec3ed956ded | 403 | }break; |
pinofal | 2:fec3ed956ded | 404 | case 'B': |
pinofal | 2:fec3ed956ded | 405 | { |
pinofal | 2:fec3ed956ded | 406 | // se ricevi il comando 'B' |
pinofal | 2:fec3ed956ded | 407 | // Vai Indietro |
pinofal | 2:fec3ed956ded | 408 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 409 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 410 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 411 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 412 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 413 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 414 | pc.printf("Indietro\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 415 | wait (2); |
pinofal | 2:fec3ed956ded | 416 | |
pinofal | 2:fec3ed956ded | 417 | // spegni |
pinofal | 2:fec3ed956ded | 418 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 419 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 420 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 421 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 422 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 423 | wait (1); |
pinofal | 2:fec3ed956ded | 424 | } break; |
pinofal | 2:fec3ed956ded | 425 | |
pinofal | 2:fec3ed956ded | 426 | case 'C': |
pinofal | 2:fec3ed956ded | 427 | { |
pinofal | 2:fec3ed956ded | 428 | // se ricevi il comando 'C' |
pinofal | 2:fec3ed956ded | 429 | // Ruota a destra |
pinofal | 2:fec3ed956ded | 430 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 431 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 432 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 433 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 434 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 435 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 436 | pc.printf("Destra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 437 | wait (3.0); |
pinofal | 2:fec3ed956ded | 438 | |
pinofal | 2:fec3ed956ded | 439 | // spegni |
pinofal | 2:fec3ed956ded | 440 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 441 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 442 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 443 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 444 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 445 | wait (1); |
pinofal | 2:fec3ed956ded | 446 | } break; |
pinofal | 2:fec3ed956ded | 447 | case 'D': |
pinofal | 2:fec3ed956ded | 448 | { |
pinofal | 2:fec3ed956ded | 449 | // se ricevi il comando 'D' |
pinofal | 2:fec3ed956ded | 450 | // Ruota a sinistra |
pinofal | 2:fec3ed956ded | 451 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 452 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 453 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 454 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 455 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 456 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 457 | pc.printf("Sinistra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 458 | wait (3.0); |
pinofal | 2:fec3ed956ded | 459 | |
pinofal | 2:fec3ed956ded | 460 | // spegni |
pinofal | 2:fec3ed956ded | 461 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 462 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 463 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 464 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 465 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 466 | wait (1); |
pinofal | 2:fec3ed956ded | 467 | } break; |
pinofal | 2:fec3ed956ded | 468 | case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti |
pinofal | 2:fec3ed956ded | 469 | { |
pinofal | 2:fec3ed956ded | 470 | // se ricevi il comando 'L' Cambia direzione di movimento delle ruote |
pinofal | 2:fec3ed956ded | 471 | PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0 |
pinofal | 2:fec3ed956ded | 472 | AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0 |
pinofal | 2:fec3ed956ded | 473 | |
pinofal | 2:fec3ed956ded | 474 | // inverti direzione di movimento |
pinofal | 2:fec3ed956ded | 475 | PostOutBI1 = !PostOutBI1; |
pinofal | 2:fec3ed956ded | 476 | PostOutBI2 = !PostOutBI2; |
pinofal | 2:fec3ed956ded | 477 | AntOutBI1 = !AntOutBI1; |
pinofal | 2:fec3ed956ded | 478 | AntOutBI2 = !AntOutBI2; |
pinofal | 2:fec3ed956ded | 479 | // ricomincia a bassa velocità |
pinofal | 2:fec3ed956ded | 480 | PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 481 | AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 482 | } break; |
pinofal | 2:fec3ed956ded | 483 | default: |
pinofal | 2:fec3ed956ded | 484 | { |
pinofal | 2:fec3ed956ded | 485 | // se ricevi comandi diversi, spegni i motori |
pinofal | 2:fec3ed956ded | 486 | // spegni |
pinofal | 2:fec3ed956ded | 487 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 488 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 489 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 490 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 491 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 492 | wait (1); |
pinofal | 2:fec3ed956ded | 493 | } break; |
pinofal | 2:fec3ed956ded | 494 | } |
pinofal | 2:fec3ed956ded | 495 | } |
pinofal | 2:fec3ed956ded | 496 | } // while(true) Test motore con PWM pilotato da BLE |
pinofal | 2:fec3ed956ded | 497 | |
pinofal | 2:fec3ed956ded | 498 | //++++++++++++ FINE Test PWM tramite BLE ++++++++++++ |
pinofal | 2:fec3ed956ded | 499 | |
pinofal | 2:fec3ed956ded | 500 | //++++++++++++ INIZIO Test Motore con PWM ++++++++++++ |
pinofal | 2:fec3ed956ded | 501 | /* |
pinofal | 2:fec3ed956ded | 502 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 503 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 504 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 505 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 506 | |
pinofal | 2:fec3ed956ded | 507 | fDutyCycle = 0.0; |
pinofal | 2:fec3ed956ded | 508 | |
pinofal | 2:fec3ed956ded | 509 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 510 | PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 511 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 512 | AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 2:fec3ed956ded | 513 | AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 514 | while(true) |
pinofal | 2:fec3ed956ded | 515 | { |
pinofal | 2:fec3ed956ded | 516 | if(cCommandBLE == 'A') |
pinofal | 2:fec3ed956ded | 517 | { |
pinofal | 2:fec3ed956ded | 518 | // Vai avanti |
pinofal | 2:fec3ed956ded | 519 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 520 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 521 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 522 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 523 | PostOutPWB.write(0.5); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 524 | AntOutPWB.write(0.5); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 525 | pc.printf("Avanti\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 526 | wait (2); |
pinofal | 2:fec3ed956ded | 527 | |
pinofal | 2:fec3ed956ded | 528 | // spegni |
pinofal | 2:fec3ed956ded | 529 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 530 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 531 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 532 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 533 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 534 | wait (1); |
pinofal | 2:fec3ed956ded | 535 | |
pinofal | 2:fec3ed956ded | 536 | // Vai Indietro |
pinofal | 2:fec3ed956ded | 537 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 538 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 539 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 540 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 541 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 542 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 543 | pc.printf("Indietro\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 544 | wait (2); |
pinofal | 2:fec3ed956ded | 545 | |
pinofal | 2:fec3ed956ded | 546 | // spegni |
pinofal | 2:fec3ed956ded | 547 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 548 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 549 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 550 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 551 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 552 | wait (1); |
pinofal | 2:fec3ed956ded | 553 | |
pinofal | 2:fec3ed956ded | 554 | // Ruota a destra |
pinofal | 2:fec3ed956ded | 555 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 556 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 557 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 558 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 559 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 560 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 561 | pc.printf("Destra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 562 | wait (2); |
pinofal | 2:fec3ed956ded | 563 | |
pinofal | 2:fec3ed956ded | 564 | // spegni |
pinofal | 2:fec3ed956ded | 565 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 566 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 567 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 568 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 569 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 570 | wait (1); |
pinofal | 2:fec3ed956ded | 571 | |
pinofal | 2:fec3ed956ded | 572 | // Ruota a sinistra |
pinofal | 2:fec3ed956ded | 573 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 574 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 575 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 576 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 577 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 578 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 579 | pc.printf("Sinistra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 580 | wait (2); |
pinofal | 2:fec3ed956ded | 581 | |
pinofal | 2:fec3ed956ded | 582 | // spegni |
pinofal | 2:fec3ed956ded | 583 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 584 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 585 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 586 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 587 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 588 | wait (1); |
pinofal | 2:fec3ed956ded | 589 | |
pinofal | 2:fec3ed956ded | 590 | } |
pinofal | 2:fec3ed956ded | 591 | } // while(true) Test motore con PWM |
pinofal | 2:fec3ed956ded | 592 | */ |
pinofal | 2:fec3ed956ded | 593 | //++++++++++++ FINE Test Motore con PWM ++++++++++++ |
pinofal | 2:fec3ed956ded | 594 | |
pinofal | 2:fec3ed956ded | 595 | //++++++++++++ INIZIO Test Motore ++++++++++++ |
pinofal | 2:fec3ed956ded | 596 | /* |
pinofal | 2:fec3ed956ded | 597 | while(1) |
pinofal | 2:fec3ed956ded | 598 | { |
pinofal | 2:fec3ed956ded | 599 | //if(myButton == 0) |
pinofal | 2:fec3ed956ded | 600 | { |
pinofal | 2:fec3ed956ded | 601 | // CW |
pinofal | 2:fec3ed956ded | 602 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 603 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 604 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 605 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 606 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 607 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 608 | pc.printf("CW\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 609 | wait (2); |
pinofal | 2:fec3ed956ded | 610 | |
pinofal | 2:fec3ed956ded | 611 | |
pinofal | 2:fec3ed956ded | 612 | // spegni |
pinofal | 2:fec3ed956ded | 613 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 614 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 615 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 616 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 617 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 618 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 619 | wait (1); |
pinofal | 2:fec3ed956ded | 620 | |
pinofal | 2:fec3ed956ded | 621 | |
pinofal | 2:fec3ed956ded | 622 | // CCW |
pinofal | 2:fec3ed956ded | 623 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 624 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 625 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 626 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 627 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 628 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 629 | wait (2); |
pinofal | 2:fec3ed956ded | 630 | pc.printf("CCW\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 631 | |
pinofal | 2:fec3ed956ded | 632 | // spegni |
pinofal | 2:fec3ed956ded | 633 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 634 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 635 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 636 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 637 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 638 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 639 | |
pinofal | 2:fec3ed956ded | 640 | wait (1); |
pinofal | 2:fec3ed956ded | 641 | |
pinofal | 2:fec3ed956ded | 642 | } |
pinofal | 2:fec3ed956ded | 643 | } // while(true) test motore |
pinofal | 2:fec3ed956ded | 644 | */ |
pinofal | 2:fec3ed956ded | 645 | //++++++++++++ FINE Test Motore ++++++++++++ |
pinofal | 2:fec3ed956ded | 646 | |
pinofal | 2:fec3ed956ded | 647 | |
pinofal | 2:fec3ed956ded | 648 | //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++ |
pinofal | 2:fec3ed956ded | 649 | /* |
pinofal | 2:fec3ed956ded | 650 | // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' |
pinofal | 2:fec3ed956ded | 651 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 652 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 653 | fDutyCycle = 0.5; |
pinofal | 2:fec3ed956ded | 654 | |
pinofal | 2:fec3ed956ded | 655 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 656 | PostOutPWB.period_ms(1000); // periodo del PWM |
pinofal | 2:fec3ed956ded | 657 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM |
pinofal | 2:fec3ed956ded | 658 | while(true) |
pinofal | 2:fec3ed956ded | 659 | { |
pinofal | 2:fec3ed956ded | 660 | // verifica se è arrivato un carattere dalla seriale del PC |
pinofal | 2:fec3ed956ded | 661 | if(pc.readable()) |
pinofal | 2:fec3ed956ded | 662 | { |
pinofal | 2:fec3ed956ded | 663 | cReadChar = pc.getc(); // Read hyperterminal |
pinofal | 2:fec3ed956ded | 664 | fDutyCycle = float(cReadChar - 0x30)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5 |
pinofal | 2:fec3ed956ded | 665 | pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle); |
pinofal | 2:fec3ed956ded | 666 | //PostOutPWB.period_ms(1000); // periodo del PWM |
pinofal | 2:fec3ed956ded | 667 | //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM |
pinofal | 2:fec3ed956ded | 668 | |
pinofal | 2:fec3ed956ded | 669 | // Inverti direzione del moto |
pinofal | 2:fec3ed956ded | 670 | //PostOutBI1 != PostOutBI1; |
pinofal | 2:fec3ed956ded | 671 | //PostOutBI2 != PostOutBI2; |
pinofal | 2:fec3ed956ded | 672 | |
pinofal | 2:fec3ed956ded | 673 | PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM |
pinofal | 2:fec3ed956ded | 674 | |
pinofal | 2:fec3ed956ded | 675 | } |
pinofal | 2:fec3ed956ded | 676 | } // while(true) test PWM tramite seriale del PC |
pinofal | 2:fec3ed956ded | 677 | */ |
pinofal | 2:fec3ed956ded | 678 | //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++ |
pinofal | 2:fec3ed956ded | 679 | |
pinofal | 2:fec3ed956ded | 680 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 681 | //++++++++++++++ FINE Ciclo test +++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 682 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 683 | |
pinofal | 2:fec3ed956ded | 684 | } // main() |