ultima versione funzionante
Dependencies: mbed
BLE-MegaPiSea_Rev2_WIP.cpp@3:b1e7eb9d61b7, 2019-07-24 (annotated)
- Committer:
- pinofal
- Date:
- Wed Jul 24 09:34:41 2019 +0000
- Revision:
- 3:b1e7eb9d61b7
- Parent:
- 2:fec3ed956ded
resettiamo il punto di partenza
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 | 3:b1e7eb9d61b7 | 35 | PwmOut PostOutPWB (PB_6); // 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 | 3:b1e7eb9d61b7 | 37 | DigitalOut PostOutBI2 (PA_7); // 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 | 3:b1e7eb9d61b7 | 89 | |
pinofal | 2:fec3ed956ded | 90 | //**********************************************/ |
pinofal | 2:fec3ed956ded | 91 | // IRQ associata a Rx da BLE |
pinofal | 2:fec3ed956ded | 92 | //**********************************************/ |
pinofal | 2:fec3ed956ded | 93 | void BLERxInterrupt(void) |
pinofal | 2:fec3ed956ded | 94 | { |
pinofal | 2:fec3ed956ded | 95 | // array per la ricezione dei messaggi da seriale |
pinofal | 2:fec3ed956ded | 96 | char cReadChar; |
pinofal | 2:fec3ed956ded | 97 | |
pinofal | 2:fec3ed956ded | 98 | //indice per i cicli |
pinofal | 2:fec3ed956ded | 99 | int nIndex; |
pinofal | 2:fec3ed956ded | 100 | |
pinofal | 2:fec3ed956ded | 101 | // array per la ricezione dei messaggi da seriale |
pinofal | 2:fec3ed956ded | 102 | char caRxPacket[8]; |
pinofal | 2:fec3ed956ded | 103 | //int nRxPacketSize; |
pinofal | 2:fec3ed956ded | 104 | |
pinofal | 2:fec3ed956ded | 105 | // coordinate cartesiane della posizione joystick |
pinofal | 2:fec3ed956ded | 106 | //float fX, fY; |
pinofal | 2:fec3ed956ded | 107 | // coordinate polari della posizione joistick |
pinofal | 2:fec3ed956ded | 108 | //float fTeta; |
pinofal | 2:fec3ed956ded | 109 | //float fRo; |
pinofal | 2:fec3ed956ded | 110 | |
pinofal | 2:fec3ed956ded | 111 | |
pinofal | 2:fec3ed956ded | 112 | |
pinofal | 2:fec3ed956ded | 113 | //pc.printf("BLE RxInterrupt: \n\r"); |
pinofal | 2:fec3ed956ded | 114 | |
pinofal | 2:fec3ed956ded | 115 | // ricevi caratteri su seriale, se disponibili |
pinofal | 2:fec3ed956ded | 116 | while((myBLE.readable())) |
pinofal | 2:fec3ed956ded | 117 | { |
pinofal | 2:fec3ed956ded | 118 | // acquisice stringa in input e relativa dimensione |
pinofal | 2:fec3ed956ded | 119 | cReadChar = myBLE.getc(); // Read character |
pinofal | 2:fec3ed956ded | 120 | if(cReadChar == 0x02) |
pinofal | 2:fec3ed956ded | 121 | { |
pinofal | 3:b1e7eb9d61b7 | 122 | pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6 |
pinofal | 3:b1e7eb9d61b7 | 123 | |
pinofal | 2:fec3ed956ded | 124 | //-- command will be 8 bytes for joystick values |
pinofal | 2:fec3ed956ded | 125 | //-- command will be 3 bytes for button change event |
pinofal | 2:fec3ed956ded | 126 | //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03) |
pinofal | 2:fec3ed956ded | 127 | |
pinofal | 2:fec3ed956ded | 128 | |
pinofal | 2:fec3ed956ded | 129 | caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX |
pinofal | 2:fec3ed956ded | 130 | cReadChar = myBLE.getc(); // legge il secondo carattere |
pinofal | 2:fec3ed956ded | 131 | if(cReadChar > 0x40) |
pinofal | 2:fec3ed956ded | 132 | { |
pinofal | 2:fec3ed956ded | 133 | // Button: |
pinofal | 2:fec3ed956ded | 134 | //-- Button events send a single character in a 3-byte packet |
pinofal | 2:fec3ed956ded | 135 | //-- 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 | 136 | //-- B2 uses "C" for changed to on, "D" for changed to off |
pinofal | 2:fec3ed956ded | 137 | //-- B3..B6 follow in order; valid button even characters are "A".."L" |
pinofal | 2:fec3ed956ded | 138 | caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP |
pinofal | 2:fec3ed956ded | 139 | caRxPacket[2] = myBLE.getc(); // legge e memorizza il terzo carattere ETX |
pinofal | 2:fec3ed956ded | 140 | // passa il comando ricevuto nella variabile globale |
pinofal | 2:fec3ed956ded | 141 | cCommandBLE = caRxPacket[1]; |
pinofal | 2:fec3ed956ded | 142 | |
pinofal | 2:fec3ed956ded | 143 | // Diagnostica |
pinofal | 2:fec3ed956ded | 144 | /* |
pinofal | 2:fec3ed956ded | 145 | 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 | 146 | */ |
pinofal | 2:fec3ed956ded | 147 | |
pinofal | 2:fec3ed956ded | 148 | } |
pinofal | 2:fec3ed956ded | 149 | else |
pinofal | 2:fec3ed956ded | 150 | { |
pinofal | 2:fec3ed956ded | 151 | // Joystick: |
pinofal | 2:fec3ed956ded | 152 | //-- 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 | 153 | //-- 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 | 154 | //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX> |
pinofal | 2:fec3ed956ded | 155 | caRxPacket[1] = cReadChar; // memorizza il secondo carattere |
pinofal | 2:fec3ed956ded | 156 | for(nIndex=2; nIndex<8; nIndex++) |
pinofal | 2:fec3ed956ded | 157 | { |
pinofal | 2:fec3ed956ded | 158 | caRxPacket[nIndex] = myBLE.getc(); |
pinofal | 2:fec3ed956ded | 159 | } |
pinofal | 2:fec3ed956ded | 160 | // dal messaggio estrae e visualizza le coordinate cartesiane |
pinofal | 2:fec3ed956ded | 161 | fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200); |
pinofal | 2:fec3ed956ded | 162 | fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200); |
pinofal | 2:fec3ed956ded | 163 | // converte la posizione del joistick in coordinate polari |
pinofal | 2:fec3ed956ded | 164 | fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo |
pinofal | 2:fec3ed956ded | 165 | if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360° |
pinofal | 2:fec3ed956ded | 166 | fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100 |
pinofal | 2:fec3ed956ded | 167 | |
pinofal | 2:fec3ed956ded | 168 | // diagnostica |
pinofal | 2:fec3ed956ded | 169 | /* |
pinofal | 2:fec3ed956ded | 170 | 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 | 171 | pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane |
pinofal | 2:fec3ed956ded | 172 | pc.printf(">: fTeta = %.2f; fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari |
pinofal | 2:fec3ed956ded | 173 | */ |
pinofal | 2:fec3ed956ded | 174 | } |
pinofal | 2:fec3ed956ded | 175 | } |
pinofal | 2:fec3ed956ded | 176 | } |
pinofal | 2:fec3ed956ded | 177 | |
pinofal | 2:fec3ed956ded | 178 | } |
pinofal | 2:fec3ed956ded | 179 | |
pinofal | 2:fec3ed956ded | 180 | /**********************************************/ |
pinofal | 2:fec3ed956ded | 181 | // IRQ associata a Rx da PC |
pinofal | 2:fec3ed956ded | 182 | //**********************************************/ |
pinofal | 2:fec3ed956ded | 183 | void pcRxInterrupt(void) |
pinofal | 2:fec3ed956ded | 184 | { |
pinofal | 2:fec3ed956ded | 185 | // array per la ricezione dei messaggi da seriale |
pinofal | 2:fec3ed956ded | 186 | char cReadChar; |
pinofal | 2:fec3ed956ded | 187 | |
pinofal | 2:fec3ed956ded | 188 | // ricevi caratteri su seriale, se disponibili |
pinofal | 2:fec3ed956ded | 189 | while((pc.readable())) |
pinofal | 2:fec3ed956ded | 190 | { |
pinofal | 2:fec3ed956ded | 191 | // acquisice stringa in input e relativa dimensione |
pinofal | 2:fec3ed956ded | 192 | cReadChar = pc.getc(); // read character from PC |
pinofal | 2:fec3ed956ded | 193 | //myBLE.putc(cReadChar); // write char to BLE |
pinofal | 2:fec3ed956ded | 194 | |
pinofal | 2:fec3ed956ded | 195 | //pc.printf("W>: 0x%02x\n\r",cReadChar); |
pinofal | 2:fec3ed956ded | 196 | if(cReadChar == '0') // se scrivo '0', invia questa stringa |
pinofal | 2:fec3ed956ded | 197 | { |
pinofal | 2:fec3ed956ded | 198 | //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 | 199 | //-- button status is binary formatted string (no indicator) |
pinofal | 2:fec3ed956ded | 200 | //-- data fields sent as strings |
pinofal | 2:fec3ed956ded | 201 | //-- send empty string to unused field (not sure if short response packet is allowed without additional testing) |
pinofal | 2:fec3ed956ded | 202 | // 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 | 203 | 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 | 204 | // diagnostica |
pinofal | 2:fec3ed956ded | 205 | /* |
pinofal | 2:fec3ed956ded | 206 | pc.printf("W>: Inviato comando a BLE\n\r"); |
pinofal | 2:fec3ed956ded | 207 | */ |
pinofal | 2:fec3ed956ded | 208 | } |
pinofal | 2:fec3ed956ded | 209 | } |
pinofal | 2:fec3ed956ded | 210 | } |
pinofal | 2:fec3ed956ded | 211 | |
pinofal | 2:fec3ed956ded | 212 | |
pinofal | 2:fec3ed956ded | 213 | /**********/ |
pinofal | 2:fec3ed956ded | 214 | /* MAIN */ |
pinofal | 2:fec3ed956ded | 215 | /**********/ |
pinofal | 2:fec3ed956ded | 216 | int main() |
pinofal | 2:fec3ed956ded | 217 | { |
pinofal | 2:fec3ed956ded | 218 | |
pinofal | 2:fec3ed956ded | 219 | // messaggio di benvenuto |
pinofal | 2:fec3ed956ded | 220 | pc.printf("\r\n******* Hallo - Exercise ********** \r\n"); |
pinofal | 2:fec3ed956ded | 221 | pc.printf("\r\n*** L476 BLE MegaPi Motor Driver ***\r\n"); |
pinofal | 2:fec3ed956ded | 222 | |
pinofal | 2:fec3ed956ded | 223 | //Inizializza pin di output per Rotating Blades |
pinofal | 2:fec3ed956ded | 224 | /* |
pinofal | 2:fec3ed956ded | 225 | OutBlades.mode(OpenDrain); |
pinofal | 2:fec3ed956ded | 226 | OutBlades.output(); |
pinofal | 2:fec3ed956ded | 227 | OutBlades.write(0); |
pinofal | 2:fec3ed956ded | 228 | nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente |
pinofal | 2:fec3ed956ded | 229 | */ |
pinofal | 2:fec3ed956ded | 230 | // inizializza variabili |
pinofal | 2:fec3ed956ded | 231 | cCommandBLE = 0; // inizialmente nessun comando da BLE |
pinofal | 2:fec3ed956ded | 232 | cOldCommandBLE = 0; // inizialmente nessun comando da BLE |
pinofal | 2:fec3ed956ded | 233 | fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 234 | fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 235 | fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 236 | fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0) |
pinofal | 2:fec3ed956ded | 237 | |
pinofal | 2:fec3ed956ded | 238 | // inizializza il BLE |
pinofal | 2:fec3ed956ded | 239 | BleRst = 0; |
pinofal | 2:fec3ed956ded | 240 | wait_ms(100); |
pinofal | 2:fec3ed956ded | 241 | BleRst = 1; |
pinofal | 2:fec3ed956ded | 242 | cCommandBLE = '0'; |
pinofal | 2:fec3ed956ded | 243 | cOldCommandBLE = '0'; |
pinofal | 2:fec3ed956ded | 244 | |
pinofal | 2:fec3ed956ded | 245 | |
pinofal | 2:fec3ed956ded | 246 | |
pinofal | 2:fec3ed956ded | 247 | // inizializza i PWM di pilotaggio dei motori Posteriore e Anteriore |
pinofal | 2:fec3ed956ded | 248 | PostOutPWB.period_us(10); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 249 | PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore) |
pinofal | 2:fec3ed956ded | 250 | AntOutPWB.period_us(10); // periodo del PWM Anteriore |
pinofal | 2:fec3ed956ded | 251 | AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore) |
pinofal | 2:fec3ed956ded | 252 | |
pinofal | 2:fec3ed956ded | 253 | |
pinofal | 2:fec3ed956ded | 254 | // Attiva la IRQ per la RX su seriale |
pinofal | 2:fec3ed956ded | 255 | myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE |
pinofal | 2:fec3ed956ded | 256 | pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC |
pinofal | 2:fec3ed956ded | 257 | |
pinofal | 2:fec3ed956ded | 258 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 259 | //++++++++++++++ INIZIO Ciclo Principale +++++++++++++ |
pinofal | 2:fec3ed956ded | 260 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 261 | /* |
pinofal | 2:fec3ed956ded | 262 | while(true) |
pinofal | 2:fec3ed956ded | 263 | { |
pinofal | 2:fec3ed956ded | 264 | //Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left: |
pinofal | 2:fec3ed956ded | 265 | //Invert X |
pinofal | 2:fec3ed956ded | 266 | //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y |
pinofal | 2:fec3ed956ded | 267 | //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X |
pinofal | 2:fec3ed956ded | 268 | //Calcola R: R = (V+W) /2 |
pinofal | 2:fec3ed956ded | 269 | //Calcola L: L= (V-W)/2 |
pinofal | 2:fec3ed956ded | 270 | //Scala i valori di L e R in base all'hardware. |
pinofal | 2:fec3ed956ded | 271 | //invia i valori al robot. |
pinofal | 2:fec3ed956ded | 272 | |
pinofal | 2:fec3ed956ded | 273 | // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote |
pinofal | 2:fec3ed956ded | 274 | if( (fX != fOldX) || (fY != fOldY)) |
pinofal | 2:fec3ed956ded | 275 | { |
pinofal | 2:fec3ed956ded | 276 | fOldX = fX; |
pinofal | 2:fec3ed956ded | 277 | fOldY = fY; |
pinofal | 2:fec3ed956ded | 278 | // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL) |
pinofal | 2:fec3ed956ded | 279 | fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio |
pinofal | 2:fec3ed956ded | 280 | fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio |
pinofal | 2:fec3ed956ded | 281 | fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100) |
pinofal | 2:fec3ed956ded | 282 | fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100) |
pinofal | 2:fec3ed956ded | 283 | |
pinofal | 2:fec3ed956ded | 284 | // diagnostica |
pinofal | 2:fec3ed956ded | 285 | pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); |
pinofal | 2:fec3ed956ded | 286 | pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); |
pinofal | 2:fec3ed956ded | 287 | pc.printf("> Velocita' Right R = %.2f\r\n", fR); |
pinofal | 2:fec3ed956ded | 288 | pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL); |
pinofal | 2:fec3ed956ded | 289 | |
pinofal | 2:fec3ed956ded | 290 | // algoritmo di movimentazione delle ruote. |
pinofal | 2:fec3ed956ded | 291 | if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore |
pinofal | 2:fec3ed956ded | 292 | { |
pinofal | 2:fec3ed956ded | 293 | fR =-fR; |
pinofal | 2:fec3ed956ded | 294 | // Vai indietro |
pinofal | 2:fec3ed956ded | 295 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 296 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 297 | } |
pinofal | 2:fec3ed956ded | 298 | else |
pinofal | 2:fec3ed956ded | 299 | { |
pinofal | 2:fec3ed956ded | 300 | |
pinofal | 2:fec3ed956ded | 301 | if(fR >0) |
pinofal | 2:fec3ed956ded | 302 | { |
pinofal | 2:fec3ed956ded | 303 | // Vai avanti |
pinofal | 2:fec3ed956ded | 304 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 305 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 306 | } |
pinofal | 2:fec3ed956ded | 307 | else |
pinofal | 2:fec3ed956ded | 308 | { |
pinofal | 2:fec3ed956ded | 309 | // spegni |
pinofal | 2:fec3ed956ded | 310 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 311 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 312 | } |
pinofal | 2:fec3ed956ded | 313 | } |
pinofal | 2:fec3ed956ded | 314 | PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore) |
pinofal | 2:fec3ed956ded | 315 | if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore |
pinofal | 2:fec3ed956ded | 316 | { |
pinofal | 2:fec3ed956ded | 317 | fL =-fL; |
pinofal | 2:fec3ed956ded | 318 | // Vai indietro |
pinofal | 2:fec3ed956ded | 319 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 320 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 321 | } |
pinofal | 2:fec3ed956ded | 322 | else |
pinofal | 2:fec3ed956ded | 323 | { |
pinofal | 2:fec3ed956ded | 324 | if(fL >0) |
pinofal | 2:fec3ed956ded | 325 | { |
pinofal | 2:fec3ed956ded | 326 | // Vai avanti |
pinofal | 2:fec3ed956ded | 327 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 328 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 329 | |
pinofal | 2:fec3ed956ded | 330 | } |
pinofal | 2:fec3ed956ded | 331 | else |
pinofal | 2:fec3ed956ded | 332 | { |
pinofal | 2:fec3ed956ded | 333 | // spegni |
pinofal | 2:fec3ed956ded | 334 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 335 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 336 | } |
pinofal | 2:fec3ed956ded | 337 | } |
pinofal | 2:fec3ed956ded | 338 | AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore) |
pinofal | 2:fec3ed956ded | 339 | } |
pinofal | 2:fec3ed956ded | 340 | } //while (true) Ciclo principale |
pinofal | 3:b1e7eb9d61b7 | 341 | */ |
pinofal | 2:fec3ed956ded | 342 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 343 | //++++++++++++++ FINE Ciclo Principale +++++++++++++ |
pinofal | 2:fec3ed956ded | 344 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 345 | |
pinofal | 2:fec3ed956ded | 346 | |
pinofal | 2:fec3ed956ded | 347 | |
pinofal | 2:fec3ed956ded | 348 | |
pinofal | 2:fec3ed956ded | 349 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 350 | //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 351 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 352 | //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 353 | |
pinofal | 3:b1e7eb9d61b7 | 354 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 355 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 356 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 357 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 358 | |
pinofal | 3:b1e7eb9d61b7 | 359 | fDutyCycle = 0.0; |
pinofal | 3:b1e7eb9d61b7 | 360 | |
pinofal | 3:b1e7eb9d61b7 | 361 | // inizializza il pin PWM |
pinofal | 3:b1e7eb9d61b7 | 362 | //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 363 | //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 364 | //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 365 | //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 366 | while(true) |
pinofal | 3:b1e7eb9d61b7 | 367 | { |
pinofal | 3:b1e7eb9d61b7 | 368 | if(cCommandBLE == 'A') |
pinofal | 3:b1e7eb9d61b7 | 369 | { |
pinofal | 3:b1e7eb9d61b7 | 370 | myLed = 0; |
pinofal | 3:b1e7eb9d61b7 | 371 | } |
pinofal | 3:b1e7eb9d61b7 | 372 | else |
pinofal | 3:b1e7eb9d61b7 | 373 | { |
pinofal | 3:b1e7eb9d61b7 | 374 | myLed = 1; |
pinofal | 3:b1e7eb9d61b7 | 375 | } |
pinofal | 3:b1e7eb9d61b7 | 376 | { |
pinofal | 3:b1e7eb9d61b7 | 377 | // Vai avanti Anteriore |
pinofal | 3:b1e7eb9d61b7 | 378 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 379 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 380 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 381 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 382 | PostOutPWB.write(0.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 383 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 384 | pc.printf("Avanti Anteriore\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 385 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 386 | |
pinofal | 3:b1e7eb9d61b7 | 387 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 388 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 389 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 390 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 391 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 392 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 393 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 394 | |
pinofal | 3:b1e7eb9d61b7 | 395 | // Vai Indietro Anteriore |
pinofal | 3:b1e7eb9d61b7 | 396 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 397 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 398 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 399 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 400 | PostOutPWB.write(0.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 401 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 402 | pc.printf("Indietro Anteriore \r\n"); |
pinofal | 3:b1e7eb9d61b7 | 403 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 404 | |
pinofal | 3:b1e7eb9d61b7 | 405 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 406 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 407 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 408 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 409 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 410 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 411 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 412 | |
pinofal | 3:b1e7eb9d61b7 | 413 | // Vai avanti Posteriore |
pinofal | 3:b1e7eb9d61b7 | 414 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 415 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 416 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 417 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 418 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 419 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 420 | pc.printf("Avanti Posteriore\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 421 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 422 | |
pinofal | 3:b1e7eb9d61b7 | 423 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 424 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 425 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 426 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 427 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 428 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 429 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 430 | |
pinofal | 3:b1e7eb9d61b7 | 431 | // Vai Indietro Posteriore |
pinofal | 3:b1e7eb9d61b7 | 432 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 433 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 434 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 435 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 436 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 437 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 438 | pc.printf("Indietro Posteriore \r\n"); |
pinofal | 3:b1e7eb9d61b7 | 439 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 440 | |
pinofal | 3:b1e7eb9d61b7 | 441 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 442 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 443 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 444 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 445 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 446 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 447 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 448 | |
pinofal | 3:b1e7eb9d61b7 | 449 | |
pinofal | 3:b1e7eb9d61b7 | 450 | |
pinofal | 3:b1e7eb9d61b7 | 451 | // Vai avanti Anteriore + Posteriore |
pinofal | 3:b1e7eb9d61b7 | 452 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 453 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 454 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 455 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 456 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 457 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 458 | pc.printf("Avanti Anteriore + Posteriore\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 459 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 460 | |
pinofal | 3:b1e7eb9d61b7 | 461 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 462 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 463 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 464 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 465 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 466 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 467 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 468 | |
pinofal | 3:b1e7eb9d61b7 | 469 | // Vai Indietro Anteriore + Posteriore |
pinofal | 3:b1e7eb9d61b7 | 470 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 471 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 472 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 473 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 474 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 475 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 476 | pc.printf("Indietro Anteriore + Posteriore \r\n"); |
pinofal | 3:b1e7eb9d61b7 | 477 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 478 | |
pinofal | 3:b1e7eb9d61b7 | 479 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 480 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 481 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 482 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 483 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 484 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 485 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 486 | |
pinofal | 3:b1e7eb9d61b7 | 487 | // Vai avanti Anteriore + Posteriore velocità ridotta |
pinofal | 3:b1e7eb9d61b7 | 488 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 489 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 490 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 491 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 492 | PostOutPWB.write(0.5); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 493 | AntOutPWB.write(0.5); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 494 | pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 495 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 496 | |
pinofal | 3:b1e7eb9d61b7 | 497 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 498 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 499 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 500 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 501 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 502 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 503 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 504 | |
pinofal | 3:b1e7eb9d61b7 | 505 | // Vai Indietro Anteriore + Posteriore velocità ridotta |
pinofal | 3:b1e7eb9d61b7 | 506 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 507 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 508 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 509 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 510 | PostOutPWB.write(0.5); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 511 | AntOutPWB.write(0.5); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 512 | pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 513 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 514 | |
pinofal | 3:b1e7eb9d61b7 | 515 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 516 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 517 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 518 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 519 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 520 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 521 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 522 | |
pinofal | 3:b1e7eb9d61b7 | 523 | |
pinofal | 3:b1e7eb9d61b7 | 524 | |
pinofal | 3:b1e7eb9d61b7 | 525 | // Ruota a destra |
pinofal | 3:b1e7eb9d61b7 | 526 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 527 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 528 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 529 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 530 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 531 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 532 | pc.printf("Ruota a Destra\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 533 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 534 | |
pinofal | 3:b1e7eb9d61b7 | 535 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 536 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 537 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 538 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 539 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 540 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 541 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 542 | |
pinofal | 3:b1e7eb9d61b7 | 543 | // Ruota a sinistra |
pinofal | 3:b1e7eb9d61b7 | 544 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 545 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 546 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 547 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 548 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 549 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 550 | pc.printf("Ruota a Sinistra\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 551 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 552 | |
pinofal | 3:b1e7eb9d61b7 | 553 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 554 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 555 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 556 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 557 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 558 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 559 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 560 | |
pinofal | 3:b1e7eb9d61b7 | 561 | } |
pinofal | 3:b1e7eb9d61b7 | 562 | } // while(true) Test motore con PWM |
pinofal | 3:b1e7eb9d61b7 | 563 | |
pinofal | 3:b1e7eb9d61b7 | 564 | //++++++++++++ FINE Test Motore con PWM ++++++++++++ |
pinofal | 2:fec3ed956ded | 565 | |
pinofal | 2:fec3ed956ded | 566 | //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 567 | /* |
pinofal | 2:fec3ed956ded | 568 | // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' |
pinofal | 2:fec3ed956ded | 569 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 570 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 571 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 572 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 573 | |
pinofal | 2:fec3ed956ded | 574 | fDutyCycle = 0.0; |
pinofal | 2:fec3ed956ded | 575 | |
pinofal | 2:fec3ed956ded | 576 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 577 | PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 578 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 579 | AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 2:fec3ed956ded | 580 | AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 581 | while(true) |
pinofal | 2:fec3ed956ded | 582 | { |
pinofal | 2:fec3ed956ded | 583 | //Parsing del comando ricevuto solo se cambia il comando. |
pinofal | 2:fec3ed956ded | 584 | if(cCommandBLE != cOldCommandBLE) |
pinofal | 2:fec3ed956ded | 585 | { |
pinofal | 2:fec3ed956ded | 586 | // ricorda il vecchio comando |
pinofal | 2:fec3ed956ded | 587 | cOldCommandBLE = cCommandBLE; |
pinofal | 2:fec3ed956ded | 588 | //cambia velocità del PWM |
pinofal | 2:fec3ed956ded | 589 | 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 | 590 | PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 591 | AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 592 | pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle); |
pinofal | 2:fec3ed956ded | 593 | //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM |
pinofal | 2:fec3ed956ded | 594 | //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0)); |
pinofal | 2:fec3ed956ded | 595 | |
pinofal | 2:fec3ed956ded | 596 | //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione |
pinofal | 2:fec3ed956ded | 597 | switch (cOldCommandBLE) // usa cOldCommandBLE. |
pinofal | 2:fec3ed956ded | 598 | { |
pinofal | 2:fec3ed956ded | 599 | case 'A': |
pinofal | 2:fec3ed956ded | 600 | { |
pinofal | 2:fec3ed956ded | 601 | // se ricevi il comando 'A' |
pinofal | 2:fec3ed956ded | 602 | // Vai avanti |
pinofal | 2:fec3ed956ded | 603 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 604 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 605 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 606 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 607 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 608 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 609 | pc.printf("Avanti\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 610 | wait (2); |
pinofal | 2:fec3ed956ded | 611 | |
pinofal | 2:fec3ed956ded | 612 | // spegni |
pinofal | 2:fec3ed956ded | 613 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 614 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 615 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 616 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 617 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 618 | wait (1); |
pinofal | 2:fec3ed956ded | 619 | }break; |
pinofal | 2:fec3ed956ded | 620 | case 'B': |
pinofal | 2:fec3ed956ded | 621 | { |
pinofal | 2:fec3ed956ded | 622 | // se ricevi il comando 'B' |
pinofal | 2:fec3ed956ded | 623 | // Vai Indietro |
pinofal | 2:fec3ed956ded | 624 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 625 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 626 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 627 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 628 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 629 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 630 | pc.printf("Indietro\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 631 | wait (2); |
pinofal | 2:fec3ed956ded | 632 | |
pinofal | 2:fec3ed956ded | 633 | // spegni |
pinofal | 2:fec3ed956ded | 634 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 635 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 636 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 637 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 638 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 639 | wait (1); |
pinofal | 2:fec3ed956ded | 640 | } break; |
pinofal | 2:fec3ed956ded | 641 | |
pinofal | 2:fec3ed956ded | 642 | case 'C': |
pinofal | 2:fec3ed956ded | 643 | { |
pinofal | 2:fec3ed956ded | 644 | // se ricevi il comando 'C' |
pinofal | 2:fec3ed956ded | 645 | // Ruota a destra |
pinofal | 2:fec3ed956ded | 646 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 647 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 648 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 649 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 650 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 651 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 652 | pc.printf("Destra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 653 | wait (3.0); |
pinofal | 2:fec3ed956ded | 654 | |
pinofal | 2:fec3ed956ded | 655 | // spegni |
pinofal | 2:fec3ed956ded | 656 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 657 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 658 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 659 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 660 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 661 | wait (1); |
pinofal | 2:fec3ed956ded | 662 | } break; |
pinofal | 2:fec3ed956ded | 663 | case 'D': |
pinofal | 2:fec3ed956ded | 664 | { |
pinofal | 2:fec3ed956ded | 665 | // se ricevi il comando 'D' |
pinofal | 2:fec3ed956ded | 666 | // Ruota a sinistra |
pinofal | 2:fec3ed956ded | 667 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 668 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 669 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 670 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 671 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 672 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 673 | pc.printf("Sinistra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 674 | wait (3.0); |
pinofal | 2:fec3ed956ded | 675 | |
pinofal | 2:fec3ed956ded | 676 | // spegni |
pinofal | 2:fec3ed956ded | 677 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 678 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 679 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 680 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 681 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 682 | wait (1); |
pinofal | 2:fec3ed956ded | 683 | } break; |
pinofal | 2:fec3ed956ded | 684 | case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti |
pinofal | 2:fec3ed956ded | 685 | { |
pinofal | 2:fec3ed956ded | 686 | // se ricevi il comando 'L' Cambia direzione di movimento delle ruote |
pinofal | 2:fec3ed956ded | 687 | PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0 |
pinofal | 2:fec3ed956ded | 688 | AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0 |
pinofal | 2:fec3ed956ded | 689 | |
pinofal | 2:fec3ed956ded | 690 | // inverti direzione di movimento |
pinofal | 2:fec3ed956ded | 691 | PostOutBI1 = !PostOutBI1; |
pinofal | 2:fec3ed956ded | 692 | PostOutBI2 = !PostOutBI2; |
pinofal | 2:fec3ed956ded | 693 | AntOutBI1 = !AntOutBI1; |
pinofal | 2:fec3ed956ded | 694 | AntOutBI2 = !AntOutBI2; |
pinofal | 2:fec3ed956ded | 695 | // ricomincia a bassa velocità |
pinofal | 2:fec3ed956ded | 696 | PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 697 | AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 698 | } break; |
pinofal | 2:fec3ed956ded | 699 | default: |
pinofal | 2:fec3ed956ded | 700 | { |
pinofal | 2:fec3ed956ded | 701 | // se ricevi comandi diversi, spegni i motori |
pinofal | 2:fec3ed956ded | 702 | // spegni |
pinofal | 2:fec3ed956ded | 703 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 704 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 705 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 706 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 707 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 708 | wait (1); |
pinofal | 2:fec3ed956ded | 709 | } break; |
pinofal | 2:fec3ed956ded | 710 | } |
pinofal | 2:fec3ed956ded | 711 | } |
pinofal | 2:fec3ed956ded | 712 | } // while(true) Test motore con PWM pilotato da BLE |
pinofal | 3:b1e7eb9d61b7 | 713 | */ |
pinofal | 2:fec3ed956ded | 714 | //++++++++++++ FINE Test PWM tramite BLE ++++++++++++ |
pinofal | 2:fec3ed956ded | 715 | |
pinofal | 3:b1e7eb9d61b7 | 716 | |
pinofal | 3:b1e7eb9d61b7 | 717 | /* |
pinofal | 3:b1e7eb9d61b7 | 718 | //++++++++++++ INIZIO Test Motore Destra o Sinistra con PWM ++++++++++++ |
pinofal | 2:fec3ed956ded | 719 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 720 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 721 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 722 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 723 | |
pinofal | 2:fec3ed956ded | 724 | fDutyCycle = 0.0; |
pinofal | 2:fec3ed956ded | 725 | |
pinofal | 2:fec3ed956ded | 726 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 727 | PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 728 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 729 | //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 730 | //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 731 | while(true) |
pinofal | 2:fec3ed956ded | 732 | { |
pinofal | 3:b1e7eb9d61b7 | 733 | //+++if(cCommandBLE == 'A') |
pinofal | 2:fec3ed956ded | 734 | { |
pinofal | 2:fec3ed956ded | 735 | // Vai avanti |
pinofal | 2:fec3ed956ded | 736 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 737 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 738 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 739 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 740 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 741 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 742 | pc.printf("Avanti\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 743 | wait (2); |
pinofal | 2:fec3ed956ded | 744 | |
pinofal | 2:fec3ed956ded | 745 | // spegni |
pinofal | 2:fec3ed956ded | 746 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 747 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 748 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 749 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 750 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 751 | wait (3); |
pinofal | 2:fec3ed956ded | 752 | |
pinofal | 2:fec3ed956ded | 753 | // Vai Indietro |
pinofal | 2:fec3ed956ded | 754 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 755 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 756 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 757 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 758 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 759 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 760 | pc.printf("Indietro\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 761 | wait (2); |
pinofal | 2:fec3ed956ded | 762 | |
pinofal | 2:fec3ed956ded | 763 | // spegni |
pinofal | 2:fec3ed956ded | 764 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 765 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 766 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 767 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 768 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 769 | wait (3); |
pinofal | 2:fec3ed956ded | 770 | |
pinofal | 2:fec3ed956ded | 771 | |
pinofal | 2:fec3ed956ded | 772 | |
pinofal | 2:fec3ed956ded | 773 | } |
pinofal | 3:b1e7eb9d61b7 | 774 | } // while(true) Test motore con PWM |
pinofal | 2:fec3ed956ded | 775 | */ |
pinofal | 3:b1e7eb9d61b7 | 776 | //++++++++++++ FINE Test Motore Destra o Sinistra con PWM ++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 777 | |
pinofal | 3:b1e7eb9d61b7 | 778 | |
pinofal | 2:fec3ed956ded | 779 | |
pinofal | 2:fec3ed956ded | 780 | //++++++++++++ INIZIO Test Motore ++++++++++++ |
pinofal | 2:fec3ed956ded | 781 | /* |
pinofal | 2:fec3ed956ded | 782 | while(1) |
pinofal | 2:fec3ed956ded | 783 | { |
pinofal | 2:fec3ed956ded | 784 | //if(myButton == 0) |
pinofal | 2:fec3ed956ded | 785 | { |
pinofal | 2:fec3ed956ded | 786 | // CW |
pinofal | 2:fec3ed956ded | 787 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 788 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 789 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 790 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 791 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 792 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 793 | pc.printf("CW\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 794 | wait (2); |
pinofal | 2:fec3ed956ded | 795 | |
pinofal | 2:fec3ed956ded | 796 | |
pinofal | 2:fec3ed956ded | 797 | // spegni |
pinofal | 2:fec3ed956ded | 798 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 799 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 800 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 801 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 802 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 803 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 804 | wait (1); |
pinofal | 2:fec3ed956ded | 805 | |
pinofal | 2:fec3ed956ded | 806 | |
pinofal | 2:fec3ed956ded | 807 | // CCW |
pinofal | 2:fec3ed956ded | 808 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 809 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 810 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 811 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 812 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 813 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 814 | wait (2); |
pinofal | 2:fec3ed956ded | 815 | pc.printf("CCW\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 816 | |
pinofal | 2:fec3ed956ded | 817 | // spegni |
pinofal | 2:fec3ed956ded | 818 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 819 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 820 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 821 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 822 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 823 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 824 | |
pinofal | 2:fec3ed956ded | 825 | wait (1); |
pinofal | 2:fec3ed956ded | 826 | |
pinofal | 2:fec3ed956ded | 827 | } |
pinofal | 2:fec3ed956ded | 828 | } // while(true) test motore |
pinofal | 2:fec3ed956ded | 829 | */ |
pinofal | 2:fec3ed956ded | 830 | //++++++++++++ FINE Test Motore ++++++++++++ |
pinofal | 2:fec3ed956ded | 831 | |
pinofal | 2:fec3ed956ded | 832 | |
pinofal | 2:fec3ed956ded | 833 | //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++ |
pinofal | 2:fec3ed956ded | 834 | /* |
pinofal | 2:fec3ed956ded | 835 | // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' |
pinofal | 2:fec3ed956ded | 836 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 837 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 838 | fDutyCycle = 0.5; |
pinofal | 2:fec3ed956ded | 839 | |
pinofal | 2:fec3ed956ded | 840 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 841 | PostOutPWB.period_ms(1000); // periodo del PWM |
pinofal | 2:fec3ed956ded | 842 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM |
pinofal | 2:fec3ed956ded | 843 | while(true) |
pinofal | 2:fec3ed956ded | 844 | { |
pinofal | 2:fec3ed956ded | 845 | // verifica se è arrivato un carattere dalla seriale del PC |
pinofal | 2:fec3ed956ded | 846 | if(pc.readable()) |
pinofal | 2:fec3ed956ded | 847 | { |
pinofal | 2:fec3ed956ded | 848 | cReadChar = pc.getc(); // Read hyperterminal |
pinofal | 2:fec3ed956ded | 849 | 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 | 850 | pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle); |
pinofal | 2:fec3ed956ded | 851 | //PostOutPWB.period_ms(1000); // periodo del PWM |
pinofal | 2:fec3ed956ded | 852 | //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM |
pinofal | 2:fec3ed956ded | 853 | |
pinofal | 2:fec3ed956ded | 854 | // Inverti direzione del moto |
pinofal | 2:fec3ed956ded | 855 | //PostOutBI1 != PostOutBI1; |
pinofal | 2:fec3ed956ded | 856 | //PostOutBI2 != PostOutBI2; |
pinofal | 2:fec3ed956ded | 857 | |
pinofal | 2:fec3ed956ded | 858 | PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM |
pinofal | 2:fec3ed956ded | 859 | |
pinofal | 2:fec3ed956ded | 860 | } |
pinofal | 2:fec3ed956ded | 861 | } // while(true) test PWM tramite seriale del PC |
pinofal | 2:fec3ed956ded | 862 | */ |
pinofal | 2:fec3ed956ded | 863 | //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++ |
pinofal | 2:fec3ed956ded | 864 | |
pinofal | 2:fec3ed956ded | 865 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 866 | //++++++++++++++ FINE Ciclo test +++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 867 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 868 | |
pinofal | 2:fec3ed956ded | 869 | } // main() |