ultima versione funzionante
Dependencies: mbed
BLE-MegaPiSea_Rev3_WIP.cpp@5:b777c4f0eb63, 2019-07-26 (annotated)
- Committer:
- pinofal
- Date:
- Fri Jul 26 07:33:36 2019 +0000
- Revision:
- 5:b777c4f0eb63
- Parent:
- 4:19d1ac0f3ec2
- Child:
- 6:92b0e17593e0
revisione ultima;
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 | 4:19d1ac0f3ec2 | 23 | DigitalOut Light(PA_0); |
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 | 5:b777c4f0eb63 | 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 | 5:b777c4f0eb63 | 272 | if(cCommandBLE != cOldCommandBLE) |
pinofal | 5:b777c4f0eb63 | 273 | { |
pinofal | 5:b777c4f0eb63 | 274 | |
pinofal | 5:b777c4f0eb63 | 275 | switch (cCommandBLE) |
pinofal | 5:b777c4f0eb63 | 276 | { |
pinofal | 5:b777c4f0eb63 | 277 | case 'A': |
pinofal | 5:b777c4f0eb63 | 278 | { |
pinofal | 5:b777c4f0eb63 | 279 | myLed = 1; |
pinofal | 5:b777c4f0eb63 | 280 | }; break; |
pinofal | 5:b777c4f0eb63 | 281 | case 'B': |
pinofal | 5:b777c4f0eb63 | 282 | { |
pinofal | 5:b777c4f0eb63 | 283 | myLed = 0; |
pinofal | 5:b777c4f0eb63 | 284 | }; break; |
pinofal | 5:b777c4f0eb63 | 285 | case 'C': |
pinofal | 5:b777c4f0eb63 | 286 | { |
pinofal | 5:b777c4f0eb63 | 287 | Light = 1; |
pinofal | 5:b777c4f0eb63 | 288 | }; break; |
pinofal | 5:b777c4f0eb63 | 289 | case 'D': |
pinofal | 5:b777c4f0eb63 | 290 | { |
pinofal | 5:b777c4f0eb63 | 291 | Light = 0; |
pinofal | 5:b777c4f0eb63 | 292 | }; break; |
pinofal | 5:b777c4f0eb63 | 293 | default: break; |
pinofal | 5:b777c4f0eb63 | 294 | } |
pinofal | 5:b777c4f0eb63 | 295 | // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica |
pinofal | 5:b777c4f0eb63 | 296 | cOldCommandBLE = cCommandBLE; |
pinofal | 5:b777c4f0eb63 | 297 | } |
pinofal | 2:fec3ed956ded | 298 | // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote |
pinofal | 2:fec3ed956ded | 299 | if( (fX != fOldX) || (fY != fOldY)) |
pinofal | 2:fec3ed956ded | 300 | { |
pinofal | 2:fec3ed956ded | 301 | fOldX = fX; |
pinofal | 2:fec3ed956ded | 302 | fOldY = fY; |
pinofal | 2:fec3ed956ded | 303 | // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL) |
pinofal | 2:fec3ed956ded | 304 | fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio |
pinofal | 2:fec3ed956ded | 305 | fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio |
pinofal | 2:fec3ed956ded | 306 | fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100) |
pinofal | 2:fec3ed956ded | 307 | fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100) |
pinofal | 2:fec3ed956ded | 308 | |
pinofal | 2:fec3ed956ded | 309 | // diagnostica |
pinofal | 2:fec3ed956ded | 310 | pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); |
pinofal | 2:fec3ed956ded | 311 | pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); |
pinofal | 2:fec3ed956ded | 312 | pc.printf("> Velocita' Right R = %.2f\r\n", fR); |
pinofal | 2:fec3ed956ded | 313 | pc.printf("> Velocita' Left L = %.2f\r\n\r\n", fL); |
pinofal | 2:fec3ed956ded | 314 | |
pinofal | 2:fec3ed956ded | 315 | // algoritmo di movimentazione delle ruote. |
pinofal | 2:fec3ed956ded | 316 | if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore |
pinofal | 2:fec3ed956ded | 317 | { |
pinofal | 2:fec3ed956ded | 318 | fR =-fR; |
pinofal | 2:fec3ed956ded | 319 | // Vai indietro |
pinofal | 2:fec3ed956ded | 320 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 321 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 322 | } |
pinofal | 2:fec3ed956ded | 323 | else |
pinofal | 2:fec3ed956ded | 324 | { |
pinofal | 2:fec3ed956ded | 325 | |
pinofal | 2:fec3ed956ded | 326 | if(fR >0) |
pinofal | 2:fec3ed956ded | 327 | { |
pinofal | 2:fec3ed956ded | 328 | // Vai avanti |
pinofal | 2:fec3ed956ded | 329 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 330 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 331 | } |
pinofal | 2:fec3ed956ded | 332 | else |
pinofal | 2:fec3ed956ded | 333 | { |
pinofal | 2:fec3ed956ded | 334 | // spegni |
pinofal | 2:fec3ed956ded | 335 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 336 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 337 | } |
pinofal | 2:fec3ed956ded | 338 | } |
pinofal | 2:fec3ed956ded | 339 | PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore) |
pinofal | 2:fec3ed956ded | 340 | if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore |
pinofal | 2:fec3ed956ded | 341 | { |
pinofal | 2:fec3ed956ded | 342 | fL =-fL; |
pinofal | 2:fec3ed956ded | 343 | // Vai indietro |
pinofal | 2:fec3ed956ded | 344 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 345 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 346 | } |
pinofal | 2:fec3ed956ded | 347 | else |
pinofal | 2:fec3ed956ded | 348 | { |
pinofal | 2:fec3ed956ded | 349 | if(fL >0) |
pinofal | 2:fec3ed956ded | 350 | { |
pinofal | 2:fec3ed956ded | 351 | // Vai avanti |
pinofal | 2:fec3ed956ded | 352 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 353 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 354 | |
pinofal | 2:fec3ed956ded | 355 | } |
pinofal | 2:fec3ed956ded | 356 | else |
pinofal | 2:fec3ed956ded | 357 | { |
pinofal | 2:fec3ed956ded | 358 | // spegni |
pinofal | 2:fec3ed956ded | 359 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 360 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 361 | } |
pinofal | 2:fec3ed956ded | 362 | } |
pinofal | 2:fec3ed956ded | 363 | AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore) |
pinofal | 2:fec3ed956ded | 364 | } |
pinofal | 2:fec3ed956ded | 365 | } //while (true) Ciclo principale |
pinofal | 5:b777c4f0eb63 | 366 | |
pinofal | 2:fec3ed956ded | 367 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 368 | //++++++++++++++ FINE Ciclo Principale +++++++++++++ |
pinofal | 2:fec3ed956ded | 369 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 370 | |
pinofal | 2:fec3ed956ded | 371 | |
pinofal | 2:fec3ed956ded | 372 | |
pinofal | 2:fec3ed956ded | 373 | |
pinofal | 2:fec3ed956ded | 374 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 375 | //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 376 | //++++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 4:19d1ac0f3ec2 | 377 | /* |
pinofal | 4:19d1ac0f3ec2 | 378 | //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++ |
pinofal | 4:19d1ac0f3ec2 | 379 | while(true) |
pinofal | 4:19d1ac0f3ec2 | 380 | { |
pinofal | 4:19d1ac0f3ec2 | 381 | if(cCommandBLE != cOldCommandBLE) |
pinofal | 4:19d1ac0f3ec2 | 382 | { |
pinofal | 4:19d1ac0f3ec2 | 383 | |
pinofal | 4:19d1ac0f3ec2 | 384 | switch (cCommandBLE) |
pinofal | 4:19d1ac0f3ec2 | 385 | { |
pinofal | 4:19d1ac0f3ec2 | 386 | case 'A': |
pinofal | 4:19d1ac0f3ec2 | 387 | { |
pinofal | 4:19d1ac0f3ec2 | 388 | myLed = 1; |
pinofal | 4:19d1ac0f3ec2 | 389 | }; break; |
pinofal | 4:19d1ac0f3ec2 | 390 | case 'B': |
pinofal | 4:19d1ac0f3ec2 | 391 | { |
pinofal | 4:19d1ac0f3ec2 | 392 | myLed = 0; |
pinofal | 4:19d1ac0f3ec2 | 393 | }; break; |
pinofal | 4:19d1ac0f3ec2 | 394 | case 'C': |
pinofal | 4:19d1ac0f3ec2 | 395 | { |
pinofal | 4:19d1ac0f3ec2 | 396 | Light = 1; |
pinofal | 4:19d1ac0f3ec2 | 397 | }; break; |
pinofal | 4:19d1ac0f3ec2 | 398 | case 'D': |
pinofal | 4:19d1ac0f3ec2 | 399 | { |
pinofal | 4:19d1ac0f3ec2 | 400 | Light = 0; |
pinofal | 4:19d1ac0f3ec2 | 401 | }; break; |
pinofal | 4:19d1ac0f3ec2 | 402 | default: break; |
pinofal | 4:19d1ac0f3ec2 | 403 | } |
pinofal | 4:19d1ac0f3ec2 | 404 | // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica |
pinofal | 4:19d1ac0f3ec2 | 405 | cOldCommandBLE = cCommandBLE; |
pinofal | 4:19d1ac0f3ec2 | 406 | } |
pinofal | 4:19d1ac0f3ec2 | 407 | |
pinofal | 4:19d1ac0f3ec2 | 408 | } // while(true) Test comandi da BLE |
pinofal | 4:19d1ac0f3ec2 | 409 | |
pinofal | 4:19d1ac0f3ec2 | 410 | //++++++++++++ FINE ricezione comandi BLE ++++++++++++ |
pinofal | 4:19d1ac0f3ec2 | 411 | */ |
pinofal | 4:19d1ac0f3ec2 | 412 | //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++ |
pinofal | 5:b777c4f0eb63 | 413 | /* |
pinofal | 3:b1e7eb9d61b7 | 414 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 415 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 416 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 417 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 418 | |
pinofal | 3:b1e7eb9d61b7 | 419 | fDutyCycle = 0.0; |
pinofal | 3:b1e7eb9d61b7 | 420 | |
pinofal | 3:b1e7eb9d61b7 | 421 | // inizializza il pin PWM |
pinofal | 3:b1e7eb9d61b7 | 422 | //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 423 | //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 424 | //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 425 | //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 426 | while(true) |
pinofal | 3:b1e7eb9d61b7 | 427 | { |
pinofal | 4:19d1ac0f3ec2 | 428 | if(cCommandBLE != cOldCommandBLE) |
pinofal | 3:b1e7eb9d61b7 | 429 | { |
pinofal | 4:19d1ac0f3ec2 | 430 | |
pinofal | 4:19d1ac0f3ec2 | 431 | if(cCommandBLE == 'A') |
pinofal | 4:19d1ac0f3ec2 | 432 | { |
pinofal | 4:19d1ac0f3ec2 | 433 | myLed = 1; |
pinofal | 4:19d1ac0f3ec2 | 434 | |
pinofal | 4:19d1ac0f3ec2 | 435 | |
pinofal | 4:19d1ac0f3ec2 | 436 | } |
pinofal | 4:19d1ac0f3ec2 | 437 | |
pinofal | 4:19d1ac0f3ec2 | 438 | else |
pinofal | 4:19d1ac0f3ec2 | 439 | { |
pinofal | 4:19d1ac0f3ec2 | 440 | myLed = 0; |
pinofal | 4:19d1ac0f3ec2 | 441 | } |
pinofal | 4:19d1ac0f3ec2 | 442 | pc.printf("Comando = %c \r\n", cCommandBLE); |
pinofal | 4:19d1ac0f3ec2 | 443 | cOldCommandBLE = cCommandBLE; |
pinofal | 3:b1e7eb9d61b7 | 444 | } |
pinofal | 3:b1e7eb9d61b7 | 445 | { |
pinofal | 3:b1e7eb9d61b7 | 446 | // Vai avanti Anteriore |
pinofal | 3:b1e7eb9d61b7 | 447 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 448 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 449 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 450 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 451 | PostOutPWB.write(0.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 452 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 453 | pc.printf("Avanti Anteriore\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 454 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 455 | |
pinofal | 3:b1e7eb9d61b7 | 456 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 457 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 458 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 459 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 460 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 461 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 462 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 463 | |
pinofal | 3:b1e7eb9d61b7 | 464 | // Vai Indietro Anteriore |
pinofal | 3:b1e7eb9d61b7 | 465 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 466 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 467 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 468 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 469 | PostOutPWB.write(0.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 470 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 471 | pc.printf("Indietro Anteriore \r\n"); |
pinofal | 3:b1e7eb9d61b7 | 472 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 473 | |
pinofal | 3:b1e7eb9d61b7 | 474 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 475 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 476 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 477 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 478 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 479 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 480 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 481 | |
pinofal | 3:b1e7eb9d61b7 | 482 | // Vai avanti Posteriore |
pinofal | 3:b1e7eb9d61b7 | 483 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 484 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 485 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 486 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 487 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 488 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 489 | pc.printf("Avanti Posteriore\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 490 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 491 | |
pinofal | 3:b1e7eb9d61b7 | 492 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 493 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 494 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 495 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 496 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 497 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 498 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 499 | |
pinofal | 3:b1e7eb9d61b7 | 500 | // Vai Indietro Posteriore |
pinofal | 3:b1e7eb9d61b7 | 501 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 502 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 503 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 504 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 505 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 506 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 507 | pc.printf("Indietro Posteriore \r\n"); |
pinofal | 3:b1e7eb9d61b7 | 508 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 509 | |
pinofal | 3:b1e7eb9d61b7 | 510 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 511 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 512 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 513 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 514 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 515 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 516 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 517 | |
pinofal | 3:b1e7eb9d61b7 | 518 | // Vai avanti Anteriore + Posteriore |
pinofal | 3:b1e7eb9d61b7 | 519 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 520 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 521 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 522 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 523 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 524 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 525 | pc.printf("Avanti Anteriore + Posteriore\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 526 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 527 | |
pinofal | 3:b1e7eb9d61b7 | 528 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 529 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 530 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 531 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 532 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 533 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 534 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 535 | |
pinofal | 3:b1e7eb9d61b7 | 536 | // Vai Indietro Anteriore + Posteriore |
pinofal | 3:b1e7eb9d61b7 | 537 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 538 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 539 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 540 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 541 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 542 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 543 | pc.printf("Indietro Anteriore + Posteriore \r\n"); |
pinofal | 3:b1e7eb9d61b7 | 544 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 545 | |
pinofal | 3:b1e7eb9d61b7 | 546 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 547 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 548 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 549 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 550 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 551 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 552 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 553 | |
pinofal | 3:b1e7eb9d61b7 | 554 | // Vai avanti Anteriore + Posteriore velocità ridotta |
pinofal | 3:b1e7eb9d61b7 | 555 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 556 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 557 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 558 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 559 | PostOutPWB.write(0.5); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 560 | AntOutPWB.write(0.5); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 561 | pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 562 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 563 | |
pinofal | 3:b1e7eb9d61b7 | 564 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 565 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 566 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 567 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 568 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 569 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 570 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 571 | |
pinofal | 3:b1e7eb9d61b7 | 572 | // Vai Indietro Anteriore + Posteriore velocità ridotta |
pinofal | 3:b1e7eb9d61b7 | 573 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 574 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 575 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 576 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 577 | PostOutPWB.write(0.5); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 578 | AntOutPWB.write(0.5); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 579 | pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 580 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 581 | |
pinofal | 3:b1e7eb9d61b7 | 582 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 583 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 584 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 585 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 586 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 587 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 588 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 589 | |
pinofal | 3:b1e7eb9d61b7 | 590 | |
pinofal | 3:b1e7eb9d61b7 | 591 | |
pinofal | 3:b1e7eb9d61b7 | 592 | // Ruota a destra |
pinofal | 3:b1e7eb9d61b7 | 593 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 594 | PostOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 595 | AntOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 596 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 597 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 598 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 599 | pc.printf("Ruota a Destra\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 600 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 601 | |
pinofal | 3:b1e7eb9d61b7 | 602 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 603 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 604 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 605 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 606 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 607 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 608 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 609 | |
pinofal | 3:b1e7eb9d61b7 | 610 | // Ruota a sinistra |
pinofal | 3:b1e7eb9d61b7 | 611 | PostOutBI1 = 1; |
pinofal | 3:b1e7eb9d61b7 | 612 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 613 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 614 | AntOutBI2 = 1; |
pinofal | 3:b1e7eb9d61b7 | 615 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 616 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 617 | pc.printf("Ruota a Sinistra\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 618 | wait (2); |
pinofal | 3:b1e7eb9d61b7 | 619 | |
pinofal | 3:b1e7eb9d61b7 | 620 | // spegni |
pinofal | 3:b1e7eb9d61b7 | 621 | PostOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 622 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 623 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 624 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 625 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 626 | wait (3); |
pinofal | 3:b1e7eb9d61b7 | 627 | |
pinofal | 3:b1e7eb9d61b7 | 628 | } |
pinofal | 3:b1e7eb9d61b7 | 629 | } // while(true) Test motore con PWM |
pinofal | 5:b777c4f0eb63 | 630 | */ |
pinofal | 3:b1e7eb9d61b7 | 631 | //++++++++++++ FINE Test Motore con PWM ++++++++++++ |
pinofal | 2:fec3ed956ded | 632 | |
pinofal | 2:fec3ed956ded | 633 | //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 634 | /* |
pinofal | 2:fec3ed956ded | 635 | // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' |
pinofal | 2:fec3ed956ded | 636 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 637 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 638 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 639 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 640 | |
pinofal | 2:fec3ed956ded | 641 | fDutyCycle = 0.0; |
pinofal | 2:fec3ed956ded | 642 | |
pinofal | 2:fec3ed956ded | 643 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 644 | PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 645 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 646 | AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 2:fec3ed956ded | 647 | AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 648 | while(true) |
pinofal | 2:fec3ed956ded | 649 | { |
pinofal | 2:fec3ed956ded | 650 | //Parsing del comando ricevuto solo se cambia il comando. |
pinofal | 2:fec3ed956ded | 651 | if(cCommandBLE != cOldCommandBLE) |
pinofal | 2:fec3ed956ded | 652 | { |
pinofal | 2:fec3ed956ded | 653 | // ricorda il vecchio comando |
pinofal | 2:fec3ed956ded | 654 | cOldCommandBLE = cCommandBLE; |
pinofal | 2:fec3ed956ded | 655 | //cambia velocità del PWM |
pinofal | 2:fec3ed956ded | 656 | 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 | 657 | PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 658 | AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 659 | pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle); |
pinofal | 2:fec3ed956ded | 660 | //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM |
pinofal | 2:fec3ed956ded | 661 | //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0)); |
pinofal | 2:fec3ed956ded | 662 | |
pinofal | 2:fec3ed956ded | 663 | //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione |
pinofal | 2:fec3ed956ded | 664 | switch (cOldCommandBLE) // usa cOldCommandBLE. |
pinofal | 2:fec3ed956ded | 665 | { |
pinofal | 2:fec3ed956ded | 666 | case 'A': |
pinofal | 2:fec3ed956ded | 667 | { |
pinofal | 2:fec3ed956ded | 668 | // se ricevi il comando 'A' |
pinofal | 2:fec3ed956ded | 669 | // Vai avanti |
pinofal | 2:fec3ed956ded | 670 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 671 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 672 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 673 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 674 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 675 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 676 | pc.printf("Avanti\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 677 | wait (2); |
pinofal | 2:fec3ed956ded | 678 | |
pinofal | 2:fec3ed956ded | 679 | // spegni |
pinofal | 2:fec3ed956ded | 680 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 681 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 682 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 683 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 684 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 685 | wait (1); |
pinofal | 2:fec3ed956ded | 686 | }break; |
pinofal | 2:fec3ed956ded | 687 | case 'B': |
pinofal | 2:fec3ed956ded | 688 | { |
pinofal | 2:fec3ed956ded | 689 | // se ricevi il comando 'B' |
pinofal | 2:fec3ed956ded | 690 | // Vai Indietro |
pinofal | 2:fec3ed956ded | 691 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 692 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 693 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 694 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 695 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 696 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 697 | pc.printf("Indietro\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 698 | wait (2); |
pinofal | 2:fec3ed956ded | 699 | |
pinofal | 2:fec3ed956ded | 700 | // spegni |
pinofal | 2:fec3ed956ded | 701 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 702 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 703 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 704 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 705 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 706 | wait (1); |
pinofal | 2:fec3ed956ded | 707 | } break; |
pinofal | 2:fec3ed956ded | 708 | |
pinofal | 2:fec3ed956ded | 709 | case 'C': |
pinofal | 2:fec3ed956ded | 710 | { |
pinofal | 2:fec3ed956ded | 711 | // se ricevi il comando 'C' |
pinofal | 2:fec3ed956ded | 712 | // Ruota a destra |
pinofal | 2:fec3ed956ded | 713 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 714 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 715 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 716 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 717 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 718 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 719 | pc.printf("Destra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 720 | wait (3.0); |
pinofal | 2:fec3ed956ded | 721 | |
pinofal | 2:fec3ed956ded | 722 | // spegni |
pinofal | 2:fec3ed956ded | 723 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 724 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 725 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 726 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 727 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 728 | wait (1); |
pinofal | 2:fec3ed956ded | 729 | } break; |
pinofal | 2:fec3ed956ded | 730 | case 'D': |
pinofal | 2:fec3ed956ded | 731 | { |
pinofal | 2:fec3ed956ded | 732 | // se ricevi il comando 'D' |
pinofal | 2:fec3ed956ded | 733 | // Ruota a sinistra |
pinofal | 2:fec3ed956ded | 734 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 735 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 736 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 737 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 738 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 739 | AntOutPWB.write(1.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 740 | pc.printf("Sinistra\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 741 | wait (3.0); |
pinofal | 2:fec3ed956ded | 742 | |
pinofal | 2:fec3ed956ded | 743 | // spegni |
pinofal | 2:fec3ed956ded | 744 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 745 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 746 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 747 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 748 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 749 | wait (1); |
pinofal | 2:fec3ed956ded | 750 | } break; |
pinofal | 2:fec3ed956ded | 751 | case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti |
pinofal | 2:fec3ed956ded | 752 | { |
pinofal | 2:fec3ed956ded | 753 | // se ricevi il comando 'L' Cambia direzione di movimento delle ruote |
pinofal | 2:fec3ed956ded | 754 | PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0 |
pinofal | 2:fec3ed956ded | 755 | AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0 |
pinofal | 2:fec3ed956ded | 756 | |
pinofal | 2:fec3ed956ded | 757 | // inverti direzione di movimento |
pinofal | 2:fec3ed956ded | 758 | PostOutBI1 = !PostOutBI1; |
pinofal | 2:fec3ed956ded | 759 | PostOutBI2 = !PostOutBI2; |
pinofal | 2:fec3ed956ded | 760 | AntOutBI1 = !AntOutBI1; |
pinofal | 2:fec3ed956ded | 761 | AntOutBI2 = !AntOutBI2; |
pinofal | 2:fec3ed956ded | 762 | // ricomincia a bassa velocità |
pinofal | 2:fec3ed956ded | 763 | PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore |
pinofal | 2:fec3ed956ded | 764 | AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 765 | } break; |
pinofal | 2:fec3ed956ded | 766 | default: |
pinofal | 2:fec3ed956ded | 767 | { |
pinofal | 2:fec3ed956ded | 768 | // se ricevi comandi diversi, spegni i motori |
pinofal | 2:fec3ed956ded | 769 | // spegni |
pinofal | 2:fec3ed956ded | 770 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 771 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 772 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 773 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 774 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 775 | wait (1); |
pinofal | 2:fec3ed956ded | 776 | } break; |
pinofal | 2:fec3ed956ded | 777 | } |
pinofal | 2:fec3ed956ded | 778 | } |
pinofal | 2:fec3ed956ded | 779 | } // while(true) Test motore con PWM pilotato da BLE |
pinofal | 3:b1e7eb9d61b7 | 780 | */ |
pinofal | 2:fec3ed956ded | 781 | //++++++++++++ FINE Test PWM tramite BLE ++++++++++++ |
pinofal | 2:fec3ed956ded | 782 | |
pinofal | 3:b1e7eb9d61b7 | 783 | |
pinofal | 3:b1e7eb9d61b7 | 784 | /* |
pinofal | 3:b1e7eb9d61b7 | 785 | //++++++++++++ INIZIO Test Motore Destra o Sinistra con PWM ++++++++++++ |
pinofal | 2:fec3ed956ded | 786 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 787 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 788 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 789 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 790 | |
pinofal | 2:fec3ed956ded | 791 | fDutyCycle = 0.0; |
pinofal | 2:fec3ed956ded | 792 | |
pinofal | 2:fec3ed956ded | 793 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 794 | PostOutPWB.period_us(100); // periodo del PWM Posteriore |
pinofal | 2:fec3ed956ded | 795 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 796 | //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore |
pinofal | 3:b1e7eb9d61b7 | 797 | //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 798 | while(true) |
pinofal | 2:fec3ed956ded | 799 | { |
pinofal | 3:b1e7eb9d61b7 | 800 | //+++if(cCommandBLE == 'A') |
pinofal | 2:fec3ed956ded | 801 | { |
pinofal | 2:fec3ed956ded | 802 | // Vai avanti |
pinofal | 2:fec3ed956ded | 803 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 804 | PostOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 805 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 806 | AntOutBI2 = 0; |
pinofal | 3:b1e7eb9d61b7 | 807 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 808 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 809 | pc.printf("Avanti\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 810 | wait (2); |
pinofal | 2:fec3ed956ded | 811 | |
pinofal | 2:fec3ed956ded | 812 | // spegni |
pinofal | 2:fec3ed956ded | 813 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 814 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 815 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 816 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 817 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 818 | wait (3); |
pinofal | 2:fec3ed956ded | 819 | |
pinofal | 2:fec3ed956ded | 820 | // Vai Indietro |
pinofal | 2:fec3ed956ded | 821 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 822 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 823 | AntOutBI1 = 0; |
pinofal | 3:b1e7eb9d61b7 | 824 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 825 | PostOutPWB.write(1.0); // duty cycle del PWM Posteriore |
pinofal | 3:b1e7eb9d61b7 | 826 | AntOutPWB.write(0.0); // duty cycle del PWM Anteriore |
pinofal | 2:fec3ed956ded | 827 | pc.printf("Indietro\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 828 | wait (2); |
pinofal | 2:fec3ed956ded | 829 | |
pinofal | 2:fec3ed956ded | 830 | // spegni |
pinofal | 2:fec3ed956ded | 831 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 832 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 833 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 834 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 835 | pc.printf("Fermo\r\n\r\n"); |
pinofal | 3:b1e7eb9d61b7 | 836 | wait (3); |
pinofal | 2:fec3ed956ded | 837 | |
pinofal | 2:fec3ed956ded | 838 | |
pinofal | 2:fec3ed956ded | 839 | |
pinofal | 2:fec3ed956ded | 840 | } |
pinofal | 3:b1e7eb9d61b7 | 841 | } // while(true) Test motore con PWM |
pinofal | 2:fec3ed956ded | 842 | */ |
pinofal | 3:b1e7eb9d61b7 | 843 | //++++++++++++ FINE Test Motore Destra o Sinistra con PWM ++++++++++++ |
pinofal | 3:b1e7eb9d61b7 | 844 | |
pinofal | 3:b1e7eb9d61b7 | 845 | |
pinofal | 2:fec3ed956ded | 846 | |
pinofal | 2:fec3ed956ded | 847 | //++++++++++++ INIZIO Test Motore ++++++++++++ |
pinofal | 2:fec3ed956ded | 848 | /* |
pinofal | 2:fec3ed956ded | 849 | while(1) |
pinofal | 2:fec3ed956ded | 850 | { |
pinofal | 2:fec3ed956ded | 851 | //if(myButton == 0) |
pinofal | 2:fec3ed956ded | 852 | { |
pinofal | 2:fec3ed956ded | 853 | // CW |
pinofal | 2:fec3ed956ded | 854 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 855 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 856 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 857 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 858 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 859 | AntOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 860 | pc.printf("CW\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 861 | wait (2); |
pinofal | 2:fec3ed956ded | 862 | |
pinofal | 2:fec3ed956ded | 863 | |
pinofal | 2:fec3ed956ded | 864 | // spegni |
pinofal | 2:fec3ed956ded | 865 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 866 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 867 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 868 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 869 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 870 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 871 | wait (1); |
pinofal | 2:fec3ed956ded | 872 | |
pinofal | 2:fec3ed956ded | 873 | |
pinofal | 2:fec3ed956ded | 874 | // CCW |
pinofal | 2:fec3ed956ded | 875 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 876 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 877 | PostOutBI2 = 1; |
pinofal | 2:fec3ed956ded | 878 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 879 | AntOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 880 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 881 | wait (2); |
pinofal | 2:fec3ed956ded | 882 | pc.printf("CCW\r\n\r\n"); |
pinofal | 2:fec3ed956ded | 883 | |
pinofal | 2:fec3ed956ded | 884 | // spegni |
pinofal | 2:fec3ed956ded | 885 | PostOutPWB = 1; |
pinofal | 2:fec3ed956ded | 886 | PostOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 887 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 888 | AntOutPWB = 1; |
pinofal | 2:fec3ed956ded | 889 | AntOutBI1 = 0; |
pinofal | 2:fec3ed956ded | 890 | AntOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 891 | |
pinofal | 2:fec3ed956ded | 892 | wait (1); |
pinofal | 2:fec3ed956ded | 893 | |
pinofal | 2:fec3ed956ded | 894 | } |
pinofal | 2:fec3ed956ded | 895 | } // while(true) test motore |
pinofal | 2:fec3ed956ded | 896 | */ |
pinofal | 2:fec3ed956ded | 897 | //++++++++++++ FINE Test Motore ++++++++++++ |
pinofal | 2:fec3ed956ded | 898 | |
pinofal | 2:fec3ed956ded | 899 | |
pinofal | 2:fec3ed956ded | 900 | //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++ |
pinofal | 2:fec3ed956ded | 901 | /* |
pinofal | 2:fec3ed956ded | 902 | // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1' |
pinofal | 2:fec3ed956ded | 903 | PostOutBI1 = 1; |
pinofal | 2:fec3ed956ded | 904 | PostOutBI2 = 0; |
pinofal | 2:fec3ed956ded | 905 | fDutyCycle = 0.5; |
pinofal | 2:fec3ed956ded | 906 | |
pinofal | 2:fec3ed956ded | 907 | // inizializza il pin PWM |
pinofal | 2:fec3ed956ded | 908 | PostOutPWB.period_ms(1000); // periodo del PWM |
pinofal | 2:fec3ed956ded | 909 | PostOutPWB.write(fDutyCycle); // duty cycle del PWM |
pinofal | 2:fec3ed956ded | 910 | while(true) |
pinofal | 2:fec3ed956ded | 911 | { |
pinofal | 2:fec3ed956ded | 912 | // verifica se è arrivato un carattere dalla seriale del PC |
pinofal | 2:fec3ed956ded | 913 | if(pc.readable()) |
pinofal | 2:fec3ed956ded | 914 | { |
pinofal | 2:fec3ed956ded | 915 | cReadChar = pc.getc(); // Read hyperterminal |
pinofal | 2:fec3ed956ded | 916 | 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 | 917 | pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle); |
pinofal | 2:fec3ed956ded | 918 | //PostOutPWB.period_ms(1000); // periodo del PWM |
pinofal | 2:fec3ed956ded | 919 | //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM |
pinofal | 2:fec3ed956ded | 920 | |
pinofal | 2:fec3ed956ded | 921 | // Inverti direzione del moto |
pinofal | 2:fec3ed956ded | 922 | //PostOutBI1 != PostOutBI1; |
pinofal | 2:fec3ed956ded | 923 | //PostOutBI2 != PostOutBI2; |
pinofal | 2:fec3ed956ded | 924 | |
pinofal | 2:fec3ed956ded | 925 | PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM |
pinofal | 2:fec3ed956ded | 926 | |
pinofal | 2:fec3ed956ded | 927 | } |
pinofal | 2:fec3ed956ded | 928 | } // while(true) test PWM tramite seriale del PC |
pinofal | 2:fec3ed956ded | 929 | */ |
pinofal | 2:fec3ed956ded | 930 | //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++ |
pinofal | 2:fec3ed956ded | 931 | |
pinofal | 2:fec3ed956ded | 932 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 933 | //++++++++++++++ FINE Ciclo test +++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 934 | //++++++++++++++++++++++++++++++++++++++++++++++++++ |
pinofal | 2:fec3ed956ded | 935 | |
pinofal | 2:fec3ed956ded | 936 | } // main() |